From: Jochen Sprickerhof Date: Mon, 19 Dec 2022 20:55:29 +0000 (+0100) Subject: New upstream version 1.13.0+dfsg X-Git-Tag: archive/raspbian/1.14.0+dfsg-2+rpi1^2~10^2~1 X-Git-Url: https://dgit.raspbian.org/%22http://www.example.com/cgi/%22/%22http:/www.example.com/cgi/%22?a=commitdiff_plain;h=177584f6fa780b92c757bcbfb818c18276a33dee;p=pcl.git New upstream version 1.13.0+dfsg --- diff --git a/.ci/azure-pipelines/azure-pipelines.yaml b/.ci/azure-pipelines/azure-pipelines.yaml index 196c3a75..8f93718d 100644 --- a/.ci/azure-pipelines/azure-pipelines.yaml +++ b/.ci/azure-pipelines/azure-pipelines.yaml @@ -20,14 +20,12 @@ resources: image: pointcloudlibrary/env:winx86 - container: winx64 image: pointcloudlibrary/env:winx64 - - container: fmt - image: pointcloudlibrary/fmt - container: env1804 image: pointcloudlibrary/env:18.04 - container: env2004 image: pointcloudlibrary/env:20.04 - - container: env2010 - image: pointcloudlibrary/env:20.10 + - container: env2204 + image: pointcloudlibrary/env:22.04 stages: - stage: formatting @@ -51,11 +49,11 @@ stages: CXX: g++ BUILD_GPU: ON CMAKE_ARGS: '-DPCL_WARNINGS_ARE_ERRORS=ON' - 20.10 GCC: # latest Ubuntu - CONTAINER: env2010 + 22.04 GCC: # latest Ubuntu + CONTAINER: env2204 CC: gcc CXX: g++ - BUILD_GPU: OFF + BUILD_GPU: OFF # There are currently incompatibilities between GCC 11.2 and CUDA 11.5 container: $[ variables['CONTAINER'] ] timeoutInMinutes: 0 variables: @@ -75,12 +73,12 @@ stages: vmImage: '$(VMIMAGE)' strategy: matrix: - Catalina 10.15: - VMIMAGE: 'macOS-10.15' - OSX_VERSION: '10.15' Big Sur 11: VMIMAGE: 'macOS-11' OSX_VERSION: '11' + Monterey 12: + VMIMAGE: 'macOS-12' + OSX_VERSION: '12' timeoutInMinutes: 0 variables: BUILD_DIR: '$(Agent.WorkFolder)/build' diff --git a/.ci/azure-pipelines/build/macos.yaml b/.ci/azure-pipelines/build/macos.yaml index 36631166..b085dba0 100644 --- a/.ci/azure-pipelines/build/macos.yaml +++ b/.ci/azure-pipelines/build/macos.yaml @@ -28,7 +28,8 @@ steps: -DBUILD_apps_cloud_composer=ON \ -DBUILD_apps_in_hand_scanner=ON \ -DBUILD_apps_modeler=ON \ - -DBUILD_apps_point_cloud_editor=ON + -DBUILD_apps_point_cloud_editor=ON \ + -DBoost_USE_DEBUG_RUNTIME=OFF displayName: 'CMake Configuration' - script: | cd $BUILD_DIR diff --git a/.ci/azure-pipelines/docs-pipeline.yaml b/.ci/azure-pipelines/docs-pipeline.yaml index e9c787cb..ed98bbc9 100644 --- a/.ci/azure-pipelines/docs-pipeline.yaml +++ b/.ci/azure-pipelines/docs-pipeline.yaml @@ -16,12 +16,10 @@ resources: stages: - build_gcc containers: - - container: fmt # for formatting.yaml - image: pointcloudlibrary/fmt - container: doc # for documentation.yaml image: pointcloudlibrary/doc - - container: env1804 # for tutorials.yaml - image: pointcloudlibrary/env:18.04 + - container: env2204 # for tutorials.yaml + image: pointcloudlibrary/env:22.04 stages: - stage: formatting diff --git a/.ci/azure-pipelines/env.yml b/.ci/azure-pipelines/env.yml index b1697c2a..b5cde4c8 100644 --- a/.ci/azure-pipelines/env.yml +++ b/.ci/azure-pipelines/env.yml @@ -42,42 +42,49 @@ jobs: strategy: matrix: Ubuntu 18.04: - CUDA_VERSION: 10.2 - UBUNTU_DISTRO: 18.04 - USE_CUDA: true - VTK_VERSION: 6 + # Test the oldest supported version of Ubuntu + UBUNTU_VERSION: 18.04 + VTK_VERSION: 7 + ENSENSOSDK_VERSION: 2.3.1570 TAG: 18.04 Ubuntu 20.04: - CUDA_VERSION: 11.2.1 - UBUNTU_DISTRO: 20.04 + UBUNTU_VERSION: 20.04 VTK_VERSION: 7 - USE_CUDA: true TAG: 20.04 - Ubuntu 20.10: - CUDA_VERSION: 11.2.1 - UBUNTU_DISTRO: 20.10 - VTK_VERSION: 7 - # nvidia-cuda docker image has not been released for 20.10 yet - USE_CUDA: "" - TAG: 20.10 - Ubuntu 21.04: - CUDA_VERSION: 11.2.1 - UBUNTU_DISTRO: 21.04 + # Test the latest LTS version of Ubuntu + Ubuntu 22.04: + UBUNTU_VERSION: 22.04 VTK_VERSION: 9 - # nvidia-cuda docker image has not been released for 21.04 yet - USE_CUDA: "" - TAG: 21.04 + TAG: 22.04 + Ubuntu 22.10: + UBUNTU_VERSION: 22.10 + USE_LATEST_CMAKE: true + VTK_VERSION: 9 + TAG: 22.10 steps: + - script: | + dockerBuildArgs="" ; \ + if [ -n "$UBUNTU_VERSION" ]; then \ + dockerBuildArgs="$dockerBuildArgs --build-arg UBUNTU_VERSION=$UBUNTU_VERSION" ; \ + fi ; \ + if [ -n "$ENSENSOSDK_VERSION" ]; then \ + dockerBuildArgs="$dockerBuildArgs --build-arg ENSENSOSDK_VERSION=$ENSENSOSDK_VERSION" ; \ + fi ; \ + if [ -n "$VTK_VERSION" ]; then \ + dockerBuildArgs="$dockerBuildArgs --build-arg VTK_VERSION=$VTK_VERSION" ; \ + fi ; \ + if [ -n "$USE_LATEST_CMAKE" ]; then \ + dockerBuildArgs="$dockerBuildArgs --build-arg USE_LATEST_CMAKE=$USE_LATEST_CMAKE" ; \ + fi + echo "##vso[task.setvariable variable=dockerBuildArgs]$dockerBuildArgs" + displayName: "Prepare docker build arguments" - task: Docker@2 displayName: "Build docker image" inputs: command: build arguments: | --no-cache - --build-arg CUDA_VERSION=$(CUDA_VERSION) - --build-arg UBUNTU_DISTRO=$(UBUNTU_DISTRO) - --build-arg USE_CUDA=$(USE_CUDA) - --build-arg VTK_VERSION=$(VTK_VERSION) + $(dockerBuildArgs) -t $(dockerHubID)/env:$(TAG) dockerfile: '$(Build.SourcesDirectory)/.dev/docker/env/Dockerfile' tags: "$(TAG)" @@ -112,7 +119,7 @@ jobs: PLATFORM: x86 TAG: winx86 GENERATOR: "'Visual Studio 16 2019' -A Win32" - VCPKGCOMMIT: 5568f110b509a9fd90711978a7cb76bae75bb092 + VCPKGCOMMIT: acc3bcf76b84ae5041c86ab55fe138ae7b8255c7 Winx64: PLATFORM: x64 TAG: winx64 diff --git a/.ci/azure-pipelines/formatting.yaml b/.ci/azure-pipelines/formatting.yaml index 342e56ff..84935030 100644 --- a/.ci/azure-pipelines/formatting.yaml +++ b/.ci/azure-pipelines/formatting.yaml @@ -2,8 +2,7 @@ jobs: - job: formatting displayName: Check code formatting pool: - vmImage: 'Ubuntu 20.04' - container: fmt + vmImage: 'ubuntu-22.04' steps: - checkout: self # find the commit hash on a quick non-forced update too diff --git a/.ci/azure-pipelines/tutorials.yaml b/.ci/azure-pipelines/tutorials.yaml index 6ca14276..9317e521 100644 --- a/.ci/azure-pipelines/tutorials.yaml +++ b/.ci/azure-pipelines/tutorials.yaml @@ -3,7 +3,7 @@ jobs: displayName: Building Tutorials pool: vmImage: 'Ubuntu 20.04' - container: env1804 + container: env2204 timeoutInMinutes: 0 variables: BUILD_DIR: '$(Agent.BuildDirectory)/build' diff --git a/.ci/azure-pipelines/ubuntu-variety.yaml b/.ci/azure-pipelines/ubuntu-variety.yaml new file mode 100644 index 00000000..0910cd6f --- /dev/null +++ b/.ci/azure-pipelines/ubuntu-variety.yaml @@ -0,0 +1,49 @@ +trigger: + branches: + include: + - master + paths: + include: + - .dev/docker/ubuntu-variety + - .ci/azure-pipelines/ubuntu-variety.yaml + +pr: + paths: + include: + - .dev/docker/ubuntu-variety + - .ci/azure-pipelines/ubuntu-variety.yaml + +schedules: +- cron: "0 0 * * 6" + displayName: "Saturday midnight build" + branches: + include: + - master + +resources: +- repo: self + +jobs: +- job: BuildUbuntuVariety + timeoutInMinutes: 360 + displayName: "BuildUbuntuVariety" + steps: + - script: | + POSSIBLE_VTK_VERSION=("7" "9") \ + POSSIBLE_CMAKE_CXX_STANDARD=("14" "17" "20") \ + POSSIBLE_CMAKE_BUILD_TYPE=("None" "Debug" "Release" "RelWithDebInfo" "MinSizeRel") \ + POSSIBLE_COMPILER_PACKAGE=("g++" "g++-10" "g++-11" "g++-12" "clang" "clang-11" "clang-12" "clang-13" "clang-14" "clang-15") \ + POSSIBLE_CMAKE_C_COMPILER=("gcc" "gcc-10" "gcc-11" "gcc-12" "clang" "clang-11" "clang-12" "clang-13" "clang-14" "clang-15") \ + POSSIBLE_CMAKE_CXX_COMPILER=("g++" "g++-10" "g++-11" "g++-12" "clang++" "clang++-11" "clang++-12" "clang++-13" "clang++-14" "clang++-15") \ + CHOSEN_COMPILER=$[RANDOM%${#POSSIBLE_COMPILER_PACKAGE[@]}] \ + dockerBuildArgs="--build-arg VTK_VERSION=${POSSIBLE_VTK_VERSION[$[RANDOM%${#POSSIBLE_VTK_VERSION[@]}]]} --build-arg CMAKE_CXX_STANDARD=${POSSIBLE_CMAKE_CXX_STANDARD[$[RANDOM%${#POSSIBLE_CMAKE_CXX_STANDARD[@]}]]} --build-arg CMAKE_BUILD_TYPE=${POSSIBLE_CMAKE_BUILD_TYPE[$[RANDOM%${#POSSIBLE_CMAKE_BUILD_TYPE[@]}]]} --build-arg COMPILER_PACKAGE=${POSSIBLE_COMPILER_PACKAGE[$CHOSEN_COMPILER]} --build-arg CMAKE_C_COMPILER=${POSSIBLE_CMAKE_C_COMPILER[$CHOSEN_COMPILER]} --build-arg CMAKE_CXX_COMPILER=${POSSIBLE_CMAKE_CXX_COMPILER[$CHOSEN_COMPILER]}" ; \ + echo "##vso[task.setvariable variable=dockerBuildArgs]$dockerBuildArgs" + displayName: "Prepare docker build arguments" + - task: Docker@2 + displayName: "Build docker image" + inputs: + command: build + arguments: | + --no-cache + $(dockerBuildArgs) + dockerfile: '$(Build.SourcesDirectory)/.dev/docker/ubuntu-variety/Dockerfile' diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..c4613478 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,5 @@ +--- +Checks: '-*,modernize-use-auto,modernize-deprecated-headers,modernize-redundant-void-arg,modernize-replace-random-shuffle,modernize-use-equals-default,modernize-use-equals-delete,modernize-use-nullptr,modernize-use-override,modernize-use-using,performance-faster-string-find,performance-for-range-copy,performance-implicit-conversion-in-loop,performance-inefficient-algorithm,performance-inefficient-vector-operation,performance-move-const-arg,performance-move-constructor-init,performance-no-automatic-move,performance-noexcept-move-constructor,performance-type-promotion-in-math-fn,cppcoreguidelines-pro-type-cstyle-cast,cppcoreguidelines-pro-type-static-cast-downcast' +WarningsAsErrors: '-*,modernize-use-auto,modernize-deprecated-headers,modernize-redundant-void-arg,modernize-replace-random-shuffle,modernize-use-equals-default,modernize-use-equals-delete,modernize-use-nullptr,modernize-use-override,modernize-use-using,performance-faster-string-find,performance-for-range-copy,performance-implicit-conversion-in-loop,performance-inefficient-algorithm,performance-inefficient-vector-operation,performance-move-const-arg,performance-move-constructor-init,performance-no-automatic-move,performance-noexcept-move-constructor,performance-type-promotion-in-math-fn,cppcoreguidelines-pro-type-cstyle-cast,cppcoreguidelines-pro-type-static-cast-downcast' +CheckOptions: +- {key: modernize-use-auto.MinTypeNameLength, value: 7} diff --git a/.dev/docker/doc/Dockerfile b/.dev/docker/doc/Dockerfile index 3f1cd54e..75dfad90 100644 --- a/.dev/docker/doc/Dockerfile +++ b/.dev/docker/doc/Dockerfile @@ -1,4 +1,4 @@ -FROM pointcloudlibrary/env:20.04 +FROM pointcloudlibrary/env:22.04 ENV DEBIAN_FRONTEND=noninteractive @@ -13,7 +13,3 @@ RUN apt-get update \ && rm -rf /var/lib/apt/lists/* RUN pip3 install Jinja2 sphinx sphinxcontrib-doxylink sphinx_rtd_theme requests grip - -# Use eps2eps to solve https://github.com/doxygen/doxygen/issues/7484 before Doxygen 1.8.18 -RUN update-alternatives --install /usr/local/bin/ps2epsi ps2epsi /usr/bin/ps2epsi 1 \ - && update-alternatives --install /usr/local/bin/ps2epsi ps2epsi /usr/bin/eps2eps 1000 diff --git a/.dev/docker/env/Dockerfile b/.dev/docker/env/Dockerfile index e2dcb662..7ac070fc 100644 --- a/.dev/docker/env/Dockerfile +++ b/.dev/docker/env/Dockerfile @@ -1,33 +1,41 @@ -# For valid combinations, check the following repo: -# https://gitlab.com/nvidia/container-images/cuda/tree/master/dist -# To enable cuda, use "--build-arg USE_CUDA=true" during image build process -ARG USE_CUDA -ARG CUDA_VERSION="9.2" -ARG UBUNTU_DISTRO="16.04" -ARG BASE_CUDA_IMAGE=${USE_CUDA:+"nvidia/cuda:${CUDA_VERSION}-devel-ubuntu${UBUNTU_DISTRO}"} -ARG BASE_IMAGE=${BASE_CUDA_IMAGE:-"ubuntu:${UBUNTU_DISTRO}"} +ARG UBUNTU_VERSION=20.04 -FROM ${BASE_IMAGE} +FROM "ubuntu:${UBUNTU_VERSION}" +# Eigen patch (https://eigen.tuxfamily.org/bz/show_bug.cgi?id=1462) to fix issue metioned +# in https://github.com/PointCloudLibrary/pcl/issues/3729 is available in Eigen 3.3.7. +# Not needed from 20.04 since it is the default version from apt +ARG EIGEN_MINIMUM_VERSION=3.3.7 + +# See https://www.optonic.com/support/download/ensenso-sdk/archiv/ for available versions +ARG ENSENSOSDK_VERSION=3.2.489 + +# See https://github.com/IntelRealSense/librealsense/tags for available tags of releases +ARG REALSENSE_VERSION=2.50.0 + +# Check https://packages.ubuntu.com/search?suite=all&arch=any&searchon=names&keywords=libvtk%20qt-dev +# for available packes for choosen UBUNTU_VERSION ARG VTK_VERSION=6 + +# Use the latest version of CMake by adding the Kitware repository if true, +# otherwise use the default version of CMake of the system. +ARG USE_LATEST_CMAKE=false + ENV DEBIAN_FRONTEND=noninteractive RUN apt-get update \ - && apt-get install -y \ - xvfb \ - cmake \ - g++ \ + && apt-get -V install -y \ + build-essential \ clang \ - wget \ + clang-tidy \ + libbenchmark-dev \ + libblas-dev \ libboost-date-time-dev \ libboost-filesystem-dev \ libboost-iostreams-dev \ - libeigen3-dev \ - libblas-dev \ libflann-dev \ libglew-dev \ libgtest-dev \ - libbenchmark-dev \ libopenni-dev \ libopenni2-dev \ libproj-dev \ @@ -36,44 +44,52 @@ RUN apt-get update \ libusb-1.0-0-dev \ libvtk${VTK_VERSION}-dev \ libvtk${VTK_VERSION}-qt-dev \ + lsb-release \ qtbase5-dev \ software-properties-common \ + wget \ + xvfb \ + && if [ "$USE_LATEST_CMAKE" = true ] ; then \ + cmake_ubuntu_version=$(lsb_release -cs) ; \ + if ! wget -q --method=HEAD "https://apt.kitware.com/ubuntu/dists/$cmake_ubuntu_version/Release"; then \ + cmake_ubuntu_version="focal" ; \ + fi ; \ + wget -qO - https://apt.kitware.com/kitware-archive.sh | bash -s -- --release $cmake_ubuntu_version ; \ + apt-get update ; \ + fi \ + && apt-get -V install -y cmake \ + && if [ "$(lsb_release -sr)" = "18.04" ]; then \ + apt-get -V install -y nvidia-cuda-toolkit g++-6 ; \ + else \ + apt-get -V install -y nvidia-cuda-toolkit-gcc ; \ + fi \ && rm -rf /var/lib/apt/lists/* -# Eigen patch (https://eigen.tuxfamily.org/bz/show_bug.cgi?id=1462) to fix issue metioned -# in https://github.com/PointCloudLibrary/pcl/issues/3729 is available in Eigen 3.3.7 -# Not needed from 20.04 since it is the default version from apt -RUN if [ `pkg-config --modversion eigen3 | cut -d. -f3` -lt 7 ]; then \ - wget -qO- https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.tar.gz | tar xz \ - && apt install -y libblas-dev \ - && cd eigen-3.3.7 \ - && mkdir build \ - && cd build \ - && cmake .. \ - && make install \ - && cd ../.. \ - && rm -rf eigen-3.3.7/ \ - && rm -f eigen-3.3.7.tar.gz ; \ - fi - -# To avoid CUDA build errors on CUDA 9.2+ GCC 7 is required -RUN if [ `gcc -dumpversion | cut -d. -f1` -lt 7 ]; then add-apt-repository ppa:ubuntu-toolchain-r/test \ - && apt update \ - && apt install g++-7 -y \ - && update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 60 --slave /usr/bin/g++ g++ /usr/bin/g++-7 \ - && update-alternatives --config gcc ; \ +# Use libeigen3-dev if it meets the minimal version. +# In most cases libeigen3-dev is already installed as a dependency of libvtk6-dev & libvtk7-dev, but not in every case (libvtk9 doesn't seem to have this dependency), +# so install it from apt if the version is sufficient. +RUN if dpkg --compare-versions $(apt-cache show --no-all-versions libeigen3-dev | grep -P -o 'Version:\s*\K.*') ge ${EIGEN_MINIMUM_VERSION}; then \ + apt-get -V install -y libeigen3-dev ; \ + else \ + wget -qO- https://gitlab.com/libeigen/eigen/-/archive/${EIGEN_MINIMUM_VERSION}/eigen-${EIGEN_MINIMUM_VERSION}.tar.gz | tar xz \ + && cd eigen-${EIGEN_MINIMUM_VERSION} \ + && mkdir build \ + && cd build \ + && cmake .. \ + && make -j$(nproc) install \ + && cd ../.. \ + && rm -rf eigen-${EIGEN_MINIMUM_VERSION}/ ; \ fi -RUN wget -qO- https://github.com/IntelRealSense/librealsense/archive/v2.23.0.tar.gz | tar xz \ - && cd librealsense-2.23.0 \ +RUN wget -qO- https://github.com/IntelRealSense/librealsense/archive/v${REALSENSE_VERSION}.tar.gz | tar xz \ + && cd librealsense-${REALSENSE_VERSION} \ && mkdir build \ && cd build \ && cmake .. -DBUILD_EXAMPLES=OFF -DBUILD_GRAPHICAL_EXAMPLES=OFF \ - && make -j2 \ - && make install \ + && make -j$(nproc) install \ && cd ../.. \ - && rm -rf librealsense-2.23.0 + && rm -rf librealsense-${REALSENSE_VERSION} -RUN wget -qO ensenso.deb https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-2.2.160-x64.deb \ +RUN wget -qO ensenso.deb https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-${ENSENSOSDK_VERSION}-x64.deb \ && dpkg -i ensenso.deb \ && rm ensenso.deb diff --git a/.dev/docker/fmt/Dockerfile b/.dev/docker/fmt/Dockerfile deleted file mode 100644 index 4535cb93..00000000 --- a/.dev/docker/fmt/Dockerfile +++ /dev/null @@ -1,12 +0,0 @@ -# Azure needs node shadow, sudo and the label -FROM node:lts-alpine - -# clang-10 needed alpine edge as of 2020-Apr-28 -RUN apk add \ - --no-cache \ - --repository=http://dl-cdn.alpinelinux.org/alpine/edge/main \ - bash clang git shadow sudo - -LABEL "com.azure.dev.pipelines.agent.handler.node.path"="/usr/local/bin/node" - -CMD [ "bash" ] diff --git a/.dev/docker/ubuntu-variety/Dockerfile b/.dev/docker/ubuntu-variety/Dockerfile new file mode 100644 index 00000000..6eb149f9 --- /dev/null +++ b/.dev/docker/ubuntu-variety/Dockerfile @@ -0,0 +1,29 @@ +# TODO maybe also rolling and latest? +FROM "ubuntu:devel" + +# TODO PCL_INDEX_SIZE and PCL_INDEX_SIGNED +# TODO test more versions of cmake, boost, vtk, eigen, qt, maybe flann, maybe other compilers? +ARG VTK_VERSION +ARG CMAKE_CXX_STANDARD +ARG CMAKE_BUILD_TYPE +ARG COMPILER_PACKAGE +ARG CMAKE_C_COMPILER +ARG CMAKE_CXX_COMPILER +RUN echo VTK_VERSION=${VTK_VERSION} CMAKE_CXX_STANDARD=${CMAKE_CXX_STANDARD} CMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} COMPILER_PACKAGE=${COMPILER_PACKAGE} CMAKE_C_COMPILER=${CMAKE_C_COMPILER} CMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt update +RUN apt install -y git cmake ${COMPILER_PACKAGE} +RUN apt install -y libeigen3-dev libflann-dev +RUN apt install -y libboost-filesystem-dev libboost-date-time-dev libboost-iostreams-dev +RUN apt install -y libgtest-dev libbenchmark-dev +RUN apt install -y qtbase5-dev libvtk${VTK_VERSION}-dev libvtk${VTK_VERSION}-qt-dev + +RUN cd \ + && git clone --depth=1 https://github.com/PointCloudLibrary/pcl \ + && mkdir pcl/build \ + && cd pcl/build \ + && cmake .. -DCMAKE_CXX_STANDARD=${CMAKE_CXX_STANDARD} -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_C_COMPILER=${CMAKE_C_COMPILER} -DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER} -DBUILD_simulation=ON -DBUILD_surface_on_nurbs=ON -DBUILD_global_tests=ON -DBUILD_benchmarks=ON -DBUILD_examples=ON -DBUILD_tools=ON -DBUILD_apps=ON -DBUILD_apps_3d_rec_framework=ON -DBUILD_apps_cloud_composer=ON -DBUILD_apps_in_hand_scanner=ON -DBUILD_apps_modeler=ON -DBUILD_apps_point_cloud_editor=ON \ + && cmake --build . -- -j2 -k \ + && cmake --build . -- tests +# TODO maybe also build tutorials? diff --git a/.dev/docker/windows/Dockerfile b/.dev/docker/windows/Dockerfile index 610587b0..44fbd8ac 100644 --- a/.dev/docker/windows/Dockerfile +++ b/.dev/docker/windows/Dockerfile @@ -30,6 +30,8 @@ RUN wget $Env:CHANNEL_BASE_URL/vs_buildtools.exe -OutFile 'C:\TEMP\vs_buildtools "C:\TEMP\VisualStudio.chman", ` "--add", ` "Microsoft.VisualStudio.Workload.VCTools", ` + "Microsoft.Net.Component.4.7.2.SDK", ` + "Microsoft.VisualStudio.Component.VC.ATLMFC", ` "--includeRecommended" ` -Wait -PassThru; ` del c:\temp\vs_buildtools.exe; @@ -46,4 +48,4 @@ COPY $PLATFORM'-windows-rel.cmake' 'c:\vcpkg\triplets\'$PLATFORM'-windows-rel.cm RUN cd .\vcpkg; ` .\bootstrap-vcpkg.bat; ` - .\vcpkg install boost flann eigen3 qhull vtk[qt,opengl] gtest benchmark --triplet $Env:PLATFORM-windows-rel --clean-after-build; + .\vcpkg install boost flann eigen3 qhull vtk[qt,opengl] gtest benchmark openni2 --triplet $Env:PLATFORM-windows-rel --clean-after-build; diff --git a/.dev/format.sh b/.dev/format.sh index 1860d850..e9bf4f33 100755 --- a/.dev/format.sh +++ b/.dev/format.sh @@ -8,7 +8,7 @@ format() { # don't use a directory with whitespace - local whitelist="apps/3d_rec_framework apps/include apps/modeler apps/src benchmarks 2d geometry ml octree simulation stereo tracking registration gpu/containers gpu/segmentation" + local whitelist="apps/3d_rec_framework apps/in_hand_scanner apps/include apps/modeler apps/src benchmarks 2d geometry ml octree simulation stereo tracking registration gpu/containers gpu/segmentation" local PCL_DIR="${2}" local formatter="${1}" diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 00000000..c1f7a566 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,13 @@ +# EditorConfig file, see https://editorconfig.org +root = true + +[*] +charset = utf-8 +indent_size = 2 +indent_style = space +insert_final_newline = true +tab_width = 2 +trim_trailing_whitespace = true + +# Visual C++ Code Style settings +cpp_generate_documentation_comments = doxygen_slash_star diff --git a/.github/ISSUE_TEMPLATE/bug-report.md b/.github/ISSUE_TEMPLATE/bug-report.md index 8bad30c5..d14e85b7 100644 --- a/.github/ISSUE_TEMPLATE/bug-report.md +++ b/.github/ISSUE_TEMPLATE/bug-report.md @@ -26,7 +26,7 @@ What happens instead of the expected behavior? **To Reproduce** -Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers. +Provide a link to a live example, or an unambiguous set of steps to reproduce this bug. A reproducible example helps to provide faster answers. If you load data e.g. from a PCD or PLY file, please provide the file. **Screenshots/Code snippets** diff --git a/.github/ISSUE_TEMPLATE/compilation-failure.md b/.github/ISSUE_TEMPLATE/compilation-failure.md index 17d00466..77771e28 100644 --- a/.github/ISSUE_TEMPLATE/compilation-failure.md +++ b/.github/ISSUE_TEMPLATE/compilation-failure.md @@ -36,7 +36,7 @@ In order to help explain your problem, please consider adding - OS: [e.g. Ubuntu 16.04] - Compiler: [:eg GCC 8.1] - PCL Version [e.g. 1.10, HEAD] (NB: please ensure you don't have multiple versions available) - - PCL Type: [Installed/Compiled from source] + - PCL Type: [e.g. Installed with VCPKG/Installed with apt/Compiled from source] If PCL was compiled from source or failure in compiling PCL itself: - GPU, Kinfu, CUDA enabled? Yes/No diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml new file mode 100755 index 00000000..86845c0c --- /dev/null +++ b/.github/workflows/clang-tidy.yml @@ -0,0 +1,23 @@ +name: clang-tidy + +on: [push, pull_request] + +jobs: + tidy: + runs-on: ubuntu-latest + container: + image: pointcloudlibrary/env:22.04 + + steps: + - uses: actions/checkout@v2 + + - name: Run clang-tidy + run: | + bash -c "$(wget -O - https://apt.llvm.org/llvm.sh)" + cmake -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_CXX_COMPILER=/usr/bin/clang-14 -DCMAKE_C_COMPILER=/usr/bin/clang-14 . \ + -DBUILD_benchmarks=ON \ + -DBUILD_examples=ON \ + -DBUILD_simulation=ON \ + -DBUILD_global_tests=ON + + run-clang-tidy -header-filter='.*' diff --git a/2d/CMakeLists.txt b/2d/CMakeLists.txt index 437d23fb..e97d4adb 100644 --- a/2d/CMakeLists.txt +++ b/2d/CMakeLists.txt @@ -4,7 +4,7 @@ set(SUBSYS_DEPS common filters) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS vtk) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS vtk) PCL_ADD_DOC("${SUBSYS_NAME}") diff --git a/2d/include/pcl/2d/keypoint.h b/2d/include/pcl/2d/keypoint.h index ca889e06..8ce1d27e 100644 --- a/2d/include/pcl/2d/keypoint.h +++ b/2d/include/pcl/2d/keypoint.h @@ -52,7 +52,7 @@ private: Convolution conv_2d; public: - Keypoint() {} + Keypoint() = default; void harrisCorner(ImageType& output, diff --git a/2d/include/pcl/2d/morphology.h b/2d/include/pcl/2d/morphology.h index b4f95639..fbe663f2 100644 --- a/2d/include/pcl/2d/morphology.h +++ b/2d/include/pcl/2d/morphology.h @@ -52,7 +52,7 @@ private: public: using PCLBase::input_; - Morphology() {} + Morphology() = default; /** \brief This function performs erosion followed by dilation. * It is useful for removing noise in the form of small blobs and patches. diff --git a/CHANGES.md b/CHANGES.md index 375412ca..c6679ae9 100644 --- a/CHANGES.md +++ b/CHANGES.md @@ -1,5 +1,198 @@ # ChangeList +## = 1.13.0 (10 December 2022) = + +### Notable changes + +**New features** *added to PCL* + +* **[ci]** Add new CI to cover uncommon configurations, running weekly [[#5208](https://github.com/PointCloudLibrary/pcl/pull/5208)] +* **[common]** Added PointCloudXYZHSVtoXYZRGB function [[#5220](https://github.com/PointCloudLibrary/pcl/pull/5220)] +* **[visualization]** Add option to enable EDL rendering. [[#4941](https://github.com/PointCloudLibrary/pcl/pull/4941)] +* **[io]** Add empty point cloud support at pcd file. [[#5400](https://github.com/PointCloudLibrary/pcl/pull/5400)] +* **[filters]** FrustumCulling: allowing infinite far plane distance [[#5433](https://github.com/PointCloudLibrary/pcl/pull/5433)] +* **[filters]** FrustumCulling: asymmetrical Field of View [[#5438](https://github.com/PointCloudLibrary/pcl/pull/5438)] +* **[filters]** Add farthest point sampling filter [[#3723](https://github.com/PointCloudLibrary/pcl/pull/3723)] +* **[sample_consensus]** Adds Ellipse3D SacModel Class [[#5512](https://github.com/PointCloudLibrary/pcl/pull/5512)] + +**Deprecation** *of public APIs, scheduled to be removed after two minor releases* + +* **[gpu]** Use C++11 `std::atomic` instead of non-standard extensions [[#4807](https://github.com/PointCloudLibrary/pcl/pull/4807)] +* **[registration]** Use likelihood instead of probability in `ndt` [[#5073](https://github.com/PointCloudLibrary/pcl/pull/5073)] +* **[gpu]** Remove hand-rolled `static_assert` [[#4797](https://github.com/PointCloudLibrary/pcl/pull/4797)] +* Deprecate remaining three boost.h headers [[#5486](https://github.com/PointCloudLibrary/pcl/pull/5486)] + +**Removal** *of the public APIs deprecated in previous releases* + +* Remove deprecated code before PCL 1.13.0 release [[#5375](https://github.com/PointCloudLibrary/pcl/pull/5375)] + +**Behavior changes** *in classes, apps, or tools* + +* **[cmake]** Drop version from pkg-config files, now accessible as `pcl_{module}.pc` [[#4977](https://github.com/PointCloudLibrary/pcl/pull/4977)] + +**API changes** *that did not go through the proper deprecation and removal cycle* + +* **[filters]** applyFilter() made protected [[#4933](https://github.com/PointCloudLibrary/pcl/pull/4933)] + +**ABI changes** *that are still API compatible* + +* **[filters]** Add `PCL_MAKE_ALIGNED_OPERATOR_NEW` to CropBox for better Eigen support [[#4962](https://github.com/PointCloudLibrary/pcl/pull/4962)] +* **[features]** Add more configuration options to GRSDEstimation [[#4852](https://github.com/PointCloudLibrary/pcl/pull/4852)] +* **[sample_consensus]** Implement `SampleConsensusModelSphere::projectPoints` properly [[#5095](https://github.com/PointCloudLibrary/pcl/pull/5095)] +* **[filters]** applyFilter() made protected [[#4933](https://github.com/PointCloudLibrary/pcl/pull/4933)] +* **[io]** Introduce buffer for texture coordinate indices in TextureMesh [[#4971](https://github.com/PointCloudLibrary/pcl/pull/4971)] +* **[filters]** Added region of interest for frustum culling [[#5136](https://github.com/PointCloudLibrary/pcl/pull/5136)] +* **[common]** constexpr constructors for point types [[#4646](https://github.com/PointCloudLibrary/pcl/pull/4646)] + +### Changes grouped by module + +#### CMake: + +* **[behavior change]** Drop version from pkg-config files, now accessible as `pcl_{module}.pc` [[#4977](https://github.com/PointCloudLibrary/pcl/pull/4977)] +* Don't require boost in pkg-config [[#5110](https://github.com/PointCloudLibrary/pcl/pull/5110)] +* Link against atomic if needed (found on armel) [[#5117](https://github.com/PointCloudLibrary/pcl/pull/5117)] +* Add more error handling to some PCL CMake functions [[#5344](https://github.com/PointCloudLibrary/pcl/pull/5344)] +* Combine `PCL_SUBSUBSYS_DEPEND` with `PCL_SUBSYS_DEPEND` [[#5387](https://github.com/PointCloudLibrary/pcl/pull/5387)] +* Allow compilation with Boost 1.80 [[#5391](https://github.com/PointCloudLibrary/pcl/pull/5391)] +* Fix issue with `TARGET_PDB_FILE` [[#5396](https://github.com/PointCloudLibrary/pcl/pull/5396)] +* Fix append multiple BOOST_ALL_NO_LIB in preprocessor macro [[#5221](https://github.com/PointCloudLibrary/pcl/pull/5221)] +* Add define for static build google benchmark. [[#5492](https://github.com/PointCloudLibrary/pcl/pull/5492)] + +#### libpcl_common: + +* Fix division and subtraction operators [[#4909](https://github.com/PointCloudLibrary/pcl/pull/4909)] +* Add `PointXY` specific behavior to `transformPointCloud()` [[#4943](https://github.com/PointCloudLibrary/pcl/pull/4943)] +* Fix division by 0 width in `PointCloud` structured `assign` [[#5113](https://github.com/PointCloudLibrary/pcl/pull/5113)] +* RangeImage: add check before accessing lookup table [[#5149](https://github.com/PointCloudLibrary/pcl/pull/5149)] +* Fix build errors [[#5155](https://github.com/PointCloudLibrary/pcl/pull/5155)] +* **[new feature]** Added PointCloudXYZHSVtoXYZRGB function [[#5220](https://github.com/PointCloudLibrary/pcl/pull/5220)] +* **[ABI break]** constexpr constructors for point types [[#4646](https://github.com/PointCloudLibrary/pcl/pull/4646)] +* Add `bool`, `std::{u}int64_t` as possible point field types [[#4133](https://github.com/PointCloudLibrary/pcl/pull/4133)] + +#### libpcl_cuda: + +* Fix build errors [[#5155](https://github.com/PointCloudLibrary/pcl/pull/5155)] +* Remove `using namespace thrust` [[#5326](https://github.com/PointCloudLibrary/pcl/pull/5326)] +* Fix linking error for Kinfu [[#5327](https://github.com/PointCloudLibrary/pcl/pull/5327)] + +#### libpcl_features: + +* **[ABI break]** Add more configuration options to GRSDEstimation [[#4852](https://github.com/PointCloudLibrary/pcl/pull/4852)] +* Fix segfault executing multiscale feature persistence [[#5109](https://github.com/PointCloudLibrary/pcl/pull/5109)] +* Remove unnecessary member in SHOTEstimationBase [[#5434](https://github.com/PointCloudLibrary/pcl/pull/5434)] + +#### libpcl_filters: + +* **[ABI break]** Add `PCL_MAKE_ALIGNED_OPERATOR_NEW` to CropBox for better Eigen support [[#4962](https://github.com/PointCloudLibrary/pcl/pull/4962)] +* **[API break][ABI break]** applyFilter() made protected [[#4933](https://github.com/PointCloudLibrary/pcl/pull/4933)] +* Use `-FLT_MAX` instead of `FLT_MIN` for minimum value [[#5119](https://github.com/PointCloudLibrary/pcl/pull/5119)] +* **[ABI break]** Added region of interest for frustum culling [[#5136](https://github.com/PointCloudLibrary/pcl/pull/5136)] +* Fix missing definition "boost::optional" [[#5213](https://github.com/PointCloudLibrary/pcl/pull/5213)] +* Fix CropHull::applyFilter3D() [[#5255](https://github.com/PointCloudLibrary/pcl/pull/5255)] +* Fix UniformSampling filter by correcting distance calculation to voxel center [[#4328](https://github.com/PointCloudLibrary/pcl/pull/4328)] +* Fix error due to multiple declarations of template member function specializations in pyramid [[#3973](https://github.com/PointCloudLibrary/pcl/pull/3973)] +* Fix segfault of NDT for sparse clouds [[#5399](https://github.com/PointCloudLibrary/pcl/pull/5399)] +* **[new feature]** FrustumCulling: allowing infinite far plane distance [[#5433](https://github.com/PointCloudLibrary/pcl/pull/5433)] +* **[new feature]** FrustumCulling: asymmetrical Field of View [[#5438](https://github.com/PointCloudLibrary/pcl/pull/5438)] +* **[new feature]** Add farthest point sampling filter [[#3723](https://github.com/PointCloudLibrary/pcl/pull/3723)] +* PassThrough: add more checks for field type and name [[#5490](https://github.com/PointCloudLibrary/pcl/pull/5490)] + +#### libpcl_gpu: + +* **[deprecation]** Use C++11 `std::atomic` instead of non-standard extensions [[#4807](https://github.com/PointCloudLibrary/pcl/pull/4807)] +* **[deprecation]** Remove hand-rolled `static_assert` [[#4797](https://github.com/PointCloudLibrary/pcl/pull/4797)] +* Remove `using namespace thrust` [[#5326](https://github.com/PointCloudLibrary/pcl/pull/5326)] +* Fix linking error for Kinfu [[#5327](https://github.com/PointCloudLibrary/pcl/pull/5327)] + +#### libpcl_io: + +* **[ABI break]** Introduce buffer for texture coordinate indices in TextureMesh [[#4971](https://github.com/PointCloudLibrary/pcl/pull/4971)] +* Fix wrong header when saving PolygonMesh to ply file [[#5169](https://github.com/PointCloudLibrary/pcl/pull/5169)] +* Reimplement boost::split and optimize tokenization [[#5285](https://github.com/PointCloudLibrary/pcl/pull/5285)] +* Fixes Crash in pcl::PLYReader::amendProperty [[#5331](https://github.com/PointCloudLibrary/pcl/pull/5331)] +* Fix multiple memory corruption errors revealed by fuzzing [[#5342](https://github.com/PointCloudLibrary/pcl/pull/5342)] +* **[new feature]** Add empty point cloud support at pcd file. [[#5400](https://github.com/PointCloudLibrary/pcl/pull/5400)] +* Fix compile issues when compiling OpenNIDriver [[#5452](https://github.com/PointCloudLibrary/pcl/pull/5452)] +* PCDReader: remove fields with zero count instead of throwing exception while reading [[#4623](https://github.com/PointCloudLibrary/pcl/pull/4623)] +* PLYReader: Return empty handler if rgb doesn't exist when trying to add alpha value [[#5415](https://github.com/PointCloudLibrary/pcl/pull/5415)] + +#### libpcl_keypoints: + +* Fix HarrisKeypoint3D::refineCorners [[#5365](https://github.com/PointCloudLibrary/pcl/pull/5365)] +* Fix OpenMP compile issue under MSVC [[#5453](https://github.com/PointCloudLibrary/pcl/pull/5453)] + +#### libpcl_octree: + +* getSize: should return 0 when data_ is invalid [[#5352](https://github.com/PointCloudLibrary/pcl/pull/5352)] +* deleteTree: max_x_ was not reset [[#5353](https://github.com/PointCloudLibrary/pcl/pull/5353)] + +#### libpcl_recognition: + +* fix quantized normals' bin boundaries not consistent in different places in linemod [[#5464](https://github.com/PointCloudLibrary/pcl/pull/5464)] +* fix linemod binindex wrong range bug in surface normal modality [[#5444](https://github.com/PointCloudLibrary/pcl/pull/5444)] + +#### libpcl_registration: + +* Fix doxygen comment blocks in `ndt.h` [[#5080](https://github.com/PointCloudLibrary/pcl/pull/5080)] +* **[deprecation]** Use likelihood instead of probability in `ndt` [[#5073](https://github.com/PointCloudLibrary/pcl/pull/5073)] +* fix: use `similarity_threshold_squared_` instead of `cardinality_` in… [[#5236](https://github.com/PointCloudLibrary/pcl/pull/5236)] +* Fix of IterativeClosestPointWithNormals shared_ptr [[#4438](https://github.com/PointCloudLibrary/pcl/pull/4438)] +* print loss as debug for TransformationEstimationSVD and TransformationEstimationPointToPlaneLLS [[#5279](https://github.com/PointCloudLibrary/pcl/pull/5279)] +* add Scalar template variable to RegistrationVisualizer [[#5290](https://github.com/PointCloudLibrary/pcl/pull/5290)] +* add Scalar template variable to NormalDistributionsTransform [[#5291](https://github.com/PointCloudLibrary/pcl/pull/5291)] +* add Scalar template variable to GeneralizedIterativeClosestPoint [[#5312](https://github.com/PointCloudLibrary/pcl/pull/5312)] +* Fix segfault of NDT for sparse clouds [[#5399](https://github.com/PointCloudLibrary/pcl/pull/5399)] +* Fix can't compile getMeanPointDensity [[#5447](https://github.com/PointCloudLibrary/pcl/pull/5447)] +* GICP: correct matrix multiplication [[#5489](https://github.com/PointCloudLibrary/pcl/pull/5489)] + +#### libpcl_sample_consensus: + +* **[ABI break]** Implement `SampleConsensusModelSphere::projectPoints` properly [[#5095](https://github.com/PointCloudLibrary/pcl/pull/5095)] +* **[new feature]** Adds Ellipse3D SacModel Class [[#5512](https://github.com/PointCloudLibrary/pcl/pull/5512)] + +#### libpcl_surface: + +* Fix mls voxel grid hashing out of bound [[#4973](https://github.com/PointCloudLibrary/pcl/pull/4973)] +* Fix nonsense code in pcl/surface/3rdparty/poisson4/sparse_matrix.hpp [[#5256](https://github.com/PointCloudLibrary/pcl/pull/5256)] +* GridProjection: scale output back to original size [[#5301](https://github.com/PointCloudLibrary/pcl/pull/5301)] +* Solve an internal compiler issue on MSVC 2022 within openNURBS [[#5463](https://github.com/PointCloudLibrary/pcl/pull/5463)] +* Fix segfault in mls::performUpsampling [[#5483](https://github.com/PointCloudLibrary/pcl/pull/5483)] + +#### libpcl_visualization: + +* Fix Bug between addText3D and QVTKWidget [[#5054](https://github.com/PointCloudLibrary/pcl/pull/5054)] +* Change static to const since it-stability is not guaranteed [[#5147](https://github.com/PointCloudLibrary/pcl/pull/5147)] +* Add missing vtk library for context2d. [[#5160](https://github.com/PointCloudLibrary/pcl/pull/5160)] +* Fix problem with spin() and spinOnce() for X Window System [[#5252](https://github.com/PointCloudLibrary/pcl/pull/5252)] +* add Scalar template variable to RegistrationVisualizer [[#5290](https://github.com/PointCloudLibrary/pcl/pull/5290)] +* Fix PCLVisualizer::addPointCloudPrincipalCurvatures [[#5369](https://github.com/PointCloudLibrary/pcl/pull/5369)] +* **[new feature]** Add option to enable EDL rendering. [[#4941](https://github.com/PointCloudLibrary/pcl/pull/4941)] +* Support handling numpad +/- key event for visualizer [[#5468](https://github.com/PointCloudLibrary/pcl/pull/5468)] +* Fix usage of dangling pointer in PCLVisualizer::getUniqueCameraFile [[#5481](https://github.com/PointCloudLibrary/pcl/pull/5481)] +* point and area picking improvement for cloud names [[#5476](https://github.com/PointCloudLibrary/pcl/pull/5476)] +* Access to a potential null pointer in interactor_style (#5503) [[#5507](https://github.com/PointCloudLibrary/pcl/pull/5507)] + +#### PCL Apps: + +* fix vtk-qt crash on macos for manual_registration app [[#5432](https://github.com/PointCloudLibrary/pcl/pull/5432)] +* Fix pcd_video_player crash on OSX [[#5421](https://github.com/PointCloudLibrary/pcl/pull/5421)] + +#### PCL Tutorials: + +* Fix alignment prerejective tutorial [[#5363](https://github.com/PointCloudLibrary/pcl/pull/5363)] + +#### PCL Tools: + +* Add check in pcd_viewer.cpp for padding fields [[#5442](https://github.com/PointCloudLibrary/pcl/pull/5442)] + +#### CI: + +* Fix benchmark compilation issue on Ubuntu 21.10 [[#5165](https://github.com/PointCloudLibrary/pcl/pull/5165)] +* **[new feature]** Add new CI to cover uncommon configurations, running weekly [[#5208](https://github.com/PointCloudLibrary/pcl/pull/5208)] +* Add clang-tidy in a GitHub workflow [[#4636](https://github.com/PointCloudLibrary/pcl/pull/4636)] +* Update vcpkg to version 2022.07.25 on x86 windows to fix libharu hash value error. [[#5418](https://github.com/PointCloudLibrary/pcl/pull/5418)] +* Install openni2 in windows dockers [[#5459](https://github.com/PointCloudLibrary/pcl/pull/5459)] + ## = 1.12.1 (2021.12.21) = This minor release brings in a lot of enhancements in CMake thanks to @larshg and @SunBlack. diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e61bfdb..17020e16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ if("${CMAKE_BUILD_TYPE}" STREQUAL "") set(CMAKE_BUILD_TYPE RelWithDebInfo CACHE STRING "build type default to RelWithDebInfo, set to Release to improve performance" FORCE) endif() -project(PCL VERSION 1.12.1) +project(PCL VERSION 1.13.0) string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) ### ---[ Find universal dependencies @@ -47,7 +47,7 @@ set(CMAKE_BUILD_TYPE "${CMAKE_BUILD_TYPE}" CACHE STRING # Compiler identification # Define a variable CMAKE_COMPILER_IS_X where X is the compiler short name. # Note: CMake automatically defines one for GNUCXX, nothing to do in this case. -if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") +if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") set(CMAKE_COMPILER_IS_CLANG 1) elseif(__COMPILER_PATHSCALE) set(CMAKE_COMPILER_IS_PATHSCALE 1) @@ -57,6 +57,20 @@ elseif(MINGW) set(CMAKE_COMPILER_IS_MINGW 1) endif() +# https://github.com/fish-shell/fish-shell/issues/5865 +include(CheckCXXSourceCompiles) +CHECK_CXX_SOURCE_COMPILES(" +#include +struct big { int foo[64]; }; +std::atomic x; +int main() { + return x.load().foo[13]; +}" +LIBATOMIC_NOT_NEEDED) +IF (NOT LIBATOMIC_NOT_NEEDED) + SET(ATOMIC_LIBRARY "atomic") +ENDIF() + # Create a variable with expected default CXX flags # This will be used further down the road to check if the user explicitly provided CXX flags if(CMAKE_COMPILER_IS_MSVC) @@ -129,7 +143,7 @@ if(CMAKE_COMPILER_IS_MSVC) add_definitions("-DBOOST_ALL_NO_LIB -D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS -DNOMINMAX -DPCL_ONLY_CORE_POINT_TYPES ${SSE_DEFINITIONS}") if("${CMAKE_CXX_FLAGS}" STREQUAL "${CMAKE_CXX_FLAGS_DEFAULT}") - string(APPEND CMAKE_CXX_FLAGS " /fp:precise /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355 ${SSE_FLAGS} ${AVX_FLAGS} /bigobj") + string(APPEND CMAKE_CXX_FLAGS " /fp:precise ${SSE_FLAGS} ${AVX_FLAGS} /bigobj") # Add extra code generation/link optimizations if(CMAKE_MSVC_CODE_LINK_OPTIMIZATION AND (NOT BUILD_CUDA) AND (NOT BUILD_GPU)) @@ -143,7 +157,21 @@ if(CMAKE_COMPILER_IS_MSVC) endif() # /MANIFEST:NO") # please, don't disable manifest generation, otherwise crash at start for vs2008 + # Disable some warnings + string(APPEND CMAKE_CXX_FLAGS " /wd4800 /wd4521 /wd4251 /wd4275 /wd4305 /wd4355") + + # Enable warnings, which are disabled by default (see https://learn.microsoft.com/de-de/cpp/preprocessor/compiler-warnings-that-are-off-by-default) + string(APPEND CMAKE_CXX_FLAGS " /w34265") + if(PCL_WARNINGS_ARE_ERRORS) + # MSVC supports external includes only since Visual Studio 2019 version 16.10.0. + # CMake supports external includes since 3.22.0 using the Ninja generator or NMake files (see https://gitlab.kitware.com/cmake/cmake/-/merge_requests/4766) + # CMake supports external includes for Visual Studio also since 3.24.0 (see https://gitlab.kitware.com/cmake/cmake/-/merge_requests/7238) + if(CMAKE_C_COMPILER_VERSION VERSION_LESS "19.29.30036.3" OR CMAKE_VERSION VERSION_LESS 3.22.0 OR (CMAKE_VERSION VERSION_LESS 3.24.0 AND CMAKE_GENERATOR MATCHES "Visual Studio")) + message(WARNING "With the used combination of compiler and CMake version it is not recommended to activate PCL_WARNINGS_ARE_ERRORS, " + "because also warnings from 3rd party components are marked as errors. It is recommended to upgrade to " + "Visual Studio 2019 version 16.10.0 and CMake 3.24.0 (or CMake 3.22.0 if using Ninja or NMake files).") + endif() string(APPEND CMAKE_CXX_FLAGS " /WX") endif() @@ -269,29 +297,14 @@ endif() if(OpenMP_FOUND) string(APPEND CMAKE_C_FLAGS " ${OpenMP_C_FLAGS}") string(APPEND CMAKE_CXX_FLAGS " ${OpenMP_CXX_FLAGS}") - if(${CMAKE_VERSION} VERSION_LESS "3.7") - message(STATUS "Found OpenMP") - else() - # We could use OpenMP_CXX_VERSION starting from CMake 3.9, but this value is only available on first run of CMake (see https://gitlab.kitware.com/cmake/cmake/issues/19150), - # so we use always OpenMP_CXX_SPEC_DATE, which is available since CMake 3.7. - message(STATUS "Found OpenMP, spec date ${OpenMP_CXX_SPEC_DATE}") - endif() - if(MSVC) - if(MSVC_VERSION EQUAL 1900) - set(OPENMP_DLL VCOMP140) - elseif(MSVC_VERSION MATCHES "^191[0-9]$") - set(OPENMP_DLL VCOMP140) - elseif(MSVC_VERSION MATCHES "^192[0-9]$") - set(OPENMP_DLL VCOMP140) - elseif(MSVC_VERSION MATCHES "^193[0-9]$") - set(OPENMP_DLL VCOMP140) - endif() - if(OPENMP_DLL) - string(APPEND CMAKE_SHARED_LINKER_FLAGS_DEBUG " /DELAYLOAD:${OPENMP_DLL}D.dll") - string(APPEND CMAKE_SHARED_LINKER_FLAGS_RELEASE " /DELAYLOAD:${OPENMP_DLL}.dll") - else() - message(WARNING "Delay loading flag for OpenMP DLL is invalid.") - endif() + + # We could use OpenMP_CXX_VERSION starting from CMake 3.9, but this value is only available on first run of CMake (see https://gitlab.kitware.com/cmake/cmake/issues/19150), + # so we use always OpenMP_CXX_SPEC_DATE, which is available since CMake 3.7. + message(STATUS "Found OpenMP, spec date ${OpenMP_CXX_SPEC_DATE}") + + if((MSVC_VERSION EQUAL 1900) OR (MSVC_VERSION MATCHES "^191[0-9]$")) + string(APPEND CMAKE_SHARED_LINKER_FLAGS_DEBUG " /DELAYLOAD:VCOMP140D.dll") + string(APPEND CMAKE_SHARED_LINKER_FLAGS_RELEASE " /DELAYLOAD:VCOMP140.dll") endif() else() message(STATUS "Not found OpenMP") @@ -301,7 +314,7 @@ endif() find_package(Threads REQUIRED) # Eigen (required) -find_package(Eigen 3.1 REQUIRED) +find_package(Eigen 3.3 REQUIRED) include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) # FLANN (required) diff --git a/PCLConfig.cmake.in b/PCLConfig.cmake.in index 27ef9807..ddf93eb6 100644 --- a/PCLConfig.cmake.in +++ b/PCLConfig.cmake.in @@ -96,7 +96,8 @@ macro(find_boost) set(Boost_ADDITIONAL_VERSIONS "@Boost_MAJOR_VERSION@.@Boost_MINOR_VERSION@.@Boost_SUBMINOR_VERSION@" "@Boost_MAJOR_VERSION@.@Boost_MINOR_VERSION@" - "1.78.0" "1.78" "1.77.0" "1.77" "1.76.0" "1.76" "1.75.0" "1.75" + "1.80.0" "1.80" + "1.79.0" "1.79" "1.78.0" "1.78" "1.77.0" "1.77" "1.76.0" "1.76" "1.75.0" "1.75" "1.74.0" "1.74" "1.73.0" "1.73" "1.72.0" "1.72" "1.71.0" "1.71" "1.70.0" "1.70" "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65") @@ -106,7 +107,7 @@ macro(find_boost) set(BOOST_INCLUDE_DIRS "${Boost_INCLUDE_DIR}") set(BOOST_LIBRARY_DIRS "${Boost_LIBRARY_DIRS}") set(BOOST_LIBRARIES ${Boost_LIBRARIES}) - if(WIN32 AND NOT MINGW) + if(WIN32 AND NOT MINGW AND NOT "${BOOST_DEFINITIONS}" MATCHES "BOOST_ALL_NO_LIB") string(APPEND BOOST_DEFINITIONS -DBOOST_ALL_NO_LIB) endif() endmacro() @@ -118,7 +119,7 @@ macro(find_eigen) elseif(NOT EIGEN_ROOT) get_filename_component(EIGEN_ROOT "@EIGEN_INCLUDE_DIRS@" ABSOLUTE) endif() - find_package(Eigen 3.1) + find_package(Eigen 3.3) endmacro() #remove this as soon as qhull is shipped with FindQhull.cmake @@ -409,7 +410,6 @@ elseif(EXISTS "${PCL_DIR}/include/pcl/pcl_config.h") # pcl_message("PCL found into a build tree.") set(PCL_CONF_INCLUDE_DIR "${PCL_DIR}/include") # for pcl_config.h set(PCL_LIBRARY_DIRS "${PCL_DIR}/@LIB_INSTALL_DIR@") - set(PCL_SOURCES_TREE "@CMAKE_SOURCE_DIR@") else() pcl_report_not_found("PCL can not be found on this machine") endif() @@ -504,7 +504,6 @@ foreach(component ${PCL_TO_FIND_COMPONENTS}) pcl/cuda/${cuda_component} pcl/cuda/${component} pcl/gpu/${gpu_component} pcl/gpu/${component} HINTS ${PCL_INCLUDE_DIRS} - "${PCL_SOURCES_TREE}" PATH_SUFFIXES ${component}/include apps/${component}/include @@ -618,16 +617,7 @@ foreach(component ${PCL_TO_FIND_COMPONENTS}) string(REGEX REPLACE "^-D" "" def3 "${def2}") list(APPEND definitions ${def3}) endforeach() - if(CMAKE_VERSION VERSION_LESS 3.3) - set_target_properties(${pcl_component} - PROPERTIES - INTERFACE_COMPILE_DEFINITIONS "${definitions}" - INTERFACE_COMPILE_OPTIONS "${PCL_COMPILE_OPTIONS}" - INTERFACE_COMPILE_FEATURES "@PCL_CXX_COMPILE_FEATURES@" - INTERFACE_INCLUDE_DIRECTORIES "${PCL_${COMPONENT}_INCLUDE_DIRS};${PCL_CONF_INCLUDE_DIR}" - INTERFACE_LINK_LIBRARIES "${PCL_${COMPONENT}_LINK_LIBRARIES}" - ) - elseif(CMAKE_VERSION VERSION_LESS 3.11) + if(CMAKE_VERSION VERSION_LESS 3.11) set_target_properties(${pcl_component} PROPERTIES INTERFACE_COMPILE_DEFINITIONS "${definitions}" diff --git a/README.md b/README.md index a895f50a..1ba99d1e 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ [![Release][release-image]][releases] [![License][license-image]][license] -[release-image]: https://img.shields.io/badge/release-1.12.1-green.svg?style=flat +[release-image]: https://img.shields.io/badge/release-1.13.0-green.svg?style=flat [releases]: https://github.com/PointCloudLibrary/pcl/releases [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat @@ -23,19 +23,19 @@ Continuous integration [ci-latest-build]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=9&branchName=master [ci-ubuntu-18.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2018.04%20GCC&label=Ubuntu%2018.04%20GCC [ci-ubuntu-20.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=Ubuntu&configuration=Ubuntu%2020.04%20Clang&label=Ubuntu%2020.04%20Clang -[ci-ubuntu-20.10]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2020.10%20GCC&label=Ubuntu%2020.10%20GCC +[ci-ubuntu-22.04]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20GCC&jobName=Ubuntu&configuration=Ubuntu%2022.04%20GCC&label=Ubuntu%2022.04%20GCC [ci-windows-x86]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20MSVC&jobName=Windows%20Build&configuration=Windows%20Build%20x86&label=Windows%20VS2019%20x86 [ci-windows-x64]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20MSVC&jobName=Windows%20Build&configuration=Windows%20Build%20x64&label=Windows%20VS2019%20x64 [ci-macos-11]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=macOS&configuration=macOS%20Big%20Sur%2011&label=macOS%20Big%20Sur%2011 -[ci-macos-10.15]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=macOS&configuration=macOS%20Catalina%2010.15&label=macOS%20Catalina%2010.15 +[ci-macos-12]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/9?branchName=master&stageName=Build%20Clang&jobName=macOS&configuration=macOS%20Monterey%2012&label=macOS%20Monterey%2012 [ci-docs]: https://dev.azure.com/PointCloudLibrary/pcl/_apis/build/status/Documentation?branchName=master [ci-latest-docs]: https://dev.azure.com/PointCloudLibrary/pcl/_build/latest?definitionId=14&branchName=master Build Platform | Status ------------------------ | ------------------------------------------------------------------------------------------------- | -Ubuntu | [![Status][ci-ubuntu-18.04]][ci-latest-build]
[![Status][ci-ubuntu-20.04]][ci-latest-build]
[![Status][ci-ubuntu-20.10]][ci-latest-build] | +Ubuntu | [![Status][ci-ubuntu-18.04]][ci-latest-build]
[![Status][ci-ubuntu-20.04]][ci-latest-build]
[![Status][ci-ubuntu-22.04]][ci-latest-build] | Windows | [![Status][ci-windows-x86]][ci-latest-build]
[![Status][ci-windows-x64]][ci-latest-build] | -macOS | [![Status][ci-macos-10.15]][ci-latest-build]
[![Status][ci-macos-11]][ci-latest-build] | +macOS | [![Status][ci-macos-11]][ci-latest-build]
[![Status][ci-macos-12]][ci-latest-build] | Documentation | [![Status][ci-docs]][ci-latest-docs] | Community diff --git a/apps/3d_rec_framework/CMakeLists.txt b/apps/3d_rec_framework/CMakeLists.txt index 219c4def..56e4ee7a 100644 --- a/apps/3d_rec_framework/CMakeLists.txt +++ b/apps/3d_rec_framework/CMakeLists.txt @@ -9,7 +9,7 @@ if(${DEFAULT} STREQUAL "TRUE") endif() PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSUBSYS_NAME} PARENT_NAME ${SUBSYS_NAME} DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) if(NOT build) return() diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h index be96e4ef..a632018d 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/global/crh_estimator.h @@ -28,7 +28,7 @@ class CRHEstimation : public GlobalEstimator { std::vector crh_histograms_; public: - CRHEstimation() {} + CRHEstimation() = default; void setFeatureEstimator( diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h index c37229b9..e777aeab 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h @@ -215,7 +215,7 @@ public: void compute(PointInTPtr& keypoints) { - if (normals_ == 0 || (normals_->size() != input_->size())) + if (normals_ == nullptr || (normals_->size() != input_->size())) PCL_WARN("SIFTSurfaceKeypointExtractor -- Normals are not valid\n"); keypoints.reset(new pcl::PointCloud); @@ -297,7 +297,7 @@ public: { keypoints.reset(new pcl::PointCloud); - if (normals_ == 0 || (normals_->size() != input_->size())) + if (normals_ == nullptr || (normals_->size() != input_->size())) PCL_WARN("HarrisKeypointExtractor -- Normals are not valid\n"); typename pcl::PointCloud::Ptr intensity_keypoints( @@ -327,7 +327,7 @@ class SUSANKeypointExtractor : public KeypointExtractor { using KeypointExtractor::radius_; public: - SUSANKeypointExtractor() {} + SUSANKeypointExtractor() = default; bool needNormals() @@ -346,7 +346,7 @@ public: { keypoints.reset(new pcl::PointCloud); - if (normals_ == 0 || (normals_->size() != input_->size())) + if (normals_ == nullptr || (normals_->size() != input_->size())) PCL_WARN("SUSANKeypointExtractor -- Normals are not valid\n"); typename pcl::PointCloud::Ptr intensity_keypoints( diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h index a8f49baa..64ec3e95 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h @@ -33,7 +33,6 @@ class PreProcessorAndNormalEstimator { pcl::Indices nn_indices(9); std::vector nn_distances(9); - float sum_distances = 0.0; std::vector avg_distances(input->size()); // Iterate through the source data set for (std::size_t i = 0; i < input->size(); ++i) { @@ -46,10 +45,12 @@ class PreProcessorAndNormalEstimator { avg_dist_neighbours /= static_cast(nn_indices.size()); avg_distances[i] = avg_dist_neighbours; - sum_distances += avg_dist_neighbours; } - std::sort(avg_distances.begin(), avg_distances.end()); + // median: nth_element is faster than sorting everything + std::nth_element(avg_distances.begin(), + avg_distances.begin() + (avg_distances.size() / 2 + 1), + avg_distances.end()); float avg = avg_distances[static_cast(avg_distances.size()) / 2 + 1]; return avg; } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h index 91b4cd9b..e2a14ac7 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pc_source/source.h @@ -194,16 +194,10 @@ public: std::shared_ptr> getModels(std::string& model_id) { - - typename std::vector::iterator it = models_->begin(); - while (it != models_->end()) { - if (model_id != (*it).id_) { - it = models_->erase(it); - } - else { - it++; - } - } + models_->erase(std::remove_if(models_->begin(), + models_->end(), + [=](ModelT& s) { return (s.id_ != model_id); }), + models_->end()); return models_; } diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h index d6554df8..85927515 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_classifier.h @@ -20,6 +20,8 @@ class PCL_EXPORTS GlobalClassifier { public: using PointInTPtr = typename pcl::PointCloud::Ptr; + virtual ~GlobalClassifier() = default; + virtual void setNN(int nn) = 0; @@ -131,8 +133,6 @@ protected: public: GlobalNNPipeline() { NN_ = 1; } - ~GlobalNNPipeline() {} - void setNN(int nn) override { diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h index ff3bde4e..c7673f24 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_crh.h @@ -153,7 +153,7 @@ public: do_CRH_ = true; } - ~GlobalNNCRHRecognizer() {} + ~GlobalNNCRHRecognizer() = default; void setNoise(float n) diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h index 54c5076c..def81b45 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/global_nn_recognizer_cvfh.h @@ -213,7 +213,7 @@ public: use_single_categories_ = false; } - ~GlobalNNCVFHRecognizer() {} + ~GlobalNNCVFHRecognizer() = default; void getDescriptorDistances(std::vector& ds) diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h index 99fa9f7c..32f9fbd7 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/pipeline/local_recognizer.h @@ -233,7 +233,7 @@ public: threshold_accept_model_hypothesis_ = t; } - ~LocalRecognitionPipeline() {} + ~LocalRecognitionPipeline() = default; void setKdtreeSplits(int n) diff --git a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h index 5f55fcbd..d0d25fa3 100644 --- a/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h +++ b/apps/3d_rec_framework/include/pcl/apps/3d_rec_framework/utils/vtk_model_sampling.h @@ -68,8 +68,7 @@ randPSurface(vtkPolyData* polydata, { float r = static_cast(uniform_deviate(rand()) * totalArea); - std::vector::iterator low = - std::lower_bound(cumulativeAreas->begin(), cumulativeAreas->end(), r); + auto low = std::lower_bound(cumulativeAreas->begin(), cumulativeAreas->end(), r); vtkIdType el = static_cast(low - cumulativeAreas->begin()); double A[3], B[3], C[3]; diff --git a/apps/3d_rec_framework/src/tools/global_classification.cpp b/apps/3d_rec_framework/src/tools/global_classification.cpp index 500bb59f..45b1b77b 100644 --- a/apps/3d_rec_framework/src/tools/global_classification.cpp +++ b/apps/3d_rec_framework/src/tools/global_classification.cpp @@ -62,9 +62,9 @@ segmentAndClassify( dps.compute_fast(clusters); dps.getIndicesClusters(indices); Eigen::Vector4f table_plane_; + dps.getTableCoefficients(table_plane_); Eigen::Vector3f normal_plane_ = Eigen::Vector3f(table_plane_[0], table_plane_[1], table_plane_[2]); - dps.getTableCoefficients(table_plane_); vis.removePointCloud("frame"); vis.addPointCloud(frame, "frame"); diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index d352ab49..85ae4a2d 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -6,7 +6,7 @@ set(SUBSYS_OPT_DEPS openni vtk ${QTX}) set(DEFAULT FALSE) set(REASON "Disabled by default") PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS ${SUBSYS_OPT_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} OPT_DEPS ${SUBSYS_OPT_DEPS}) if(NOT build) return() diff --git a/apps/cloud_composer/CMakeLists.txt b/apps/cloud_composer/CMakeLists.txt index c9223a9e..24196bcc 100644 --- a/apps/cloud_composer/CMakeLists.txt +++ b/apps/cloud_composer/CMakeLists.txt @@ -23,7 +23,7 @@ if("${DEFAULT}" STREQUAL "TRUE") endif() PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSUBSYS_NAME} PARENT_NAME ${SUBSYS_NAME} DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) PCL_ADD_DOC(${SUBSUBSYS_NAME}) diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_viewer.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_viewer.h index 62ebc4f2..df10dc1f 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_viewer.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/cloud_viewer.h @@ -59,7 +59,6 @@ namespace pcl public: CloudViewer (QWidget* parent = nullptr); - ~CloudViewer(); ProjectModel* getModel () const; public Q_SLOTS: diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/item_inspector.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/item_inspector.h index befb2997..dcf7f78c 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/item_inspector.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/item_inspector.h @@ -58,7 +58,6 @@ namespace pcl Q_OBJECT public: ItemInspector (QWidget* parent = nullptr); - ~ItemInspector(); public Q_SLOTS: void diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h index a54f7fb2..aabf5aab 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/cloud_item.h @@ -87,7 +87,6 @@ namespace pcl bool make_templated_cloud = true); CloudItem (const CloudItem& to_copy); - ~CloudItem (); /** \brief This creates a CloudItem from a templated cloud type */ template diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h index 5f3ac452..5a634272 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/fpfh_item.h @@ -57,7 +57,6 @@ namespace pcl const pcl::PointCloud::Ptr& fpfh_ptr, double radius); FPFHItem (const FPFHItem& to_copy); - ~FPFHItem (); inline int type () const override { return FPFH_ITEM; } diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/normals_item.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/normals_item.h index 0c8d2706..73c687bd 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/items/normals_item.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/items/normals_item.h @@ -55,7 +55,6 @@ namespace pcl const pcl::PointCloud::Ptr& normals_ptr, double radius); NormalsItem (const NormalsItem& to_copy); - ~NormalsItem (); inline int type () const override { return NORMALS_ITEM; } diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/merge_selection.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/merge_selection.h index 6a0d4e44..af3d7afa 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/merge_selection.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/merge_selection.h @@ -48,7 +48,6 @@ namespace pcl Q_OBJECT public: MergeSelection (QMap selected_item_index_map, QObject* parent = nullptr); - ~MergeSelection (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h index ecc1a390..2477b33b 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/click_trackball_interactor_style.h @@ -53,8 +53,6 @@ namespace pcl vtkTypeMacro(ClickTrackballStyleInteractor,vtkInteractorStyleTrackballActor); ClickTrackballStyleInteractor (); - - ~ClickTrackballStyleInteractor (); /** \brief Pass a pointer to the actor map * \param[in] actors the actor map that will be used with this style diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h index 1dd72c1d..1f4fcd17 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/interactor_style_switch.h @@ -89,7 +89,6 @@ namespace pcl vtkTypeMacro(InteractorStyleSwitch, vtkInteractorStyle); InteractorStyleSwitch(); - ~InteractorStyleSwitch(); void SetInteractor(vtkRenderWindowInteractor *iren) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h index 49ef9008..73efe79c 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/manipulation_event.h @@ -52,8 +52,6 @@ namespace pcl ManipulationEvent () {} - ~ManipulationEvent (); - void addManipulation (const QString& id, const vtkSmartPointer& start, const vtkSmartPointer& end); diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h index a89982b9..bed9b5d5 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/rectangular_frustum_selector.h @@ -55,8 +55,6 @@ namespace pcl vtkTypeMacro(RectangularFrustumSelector,vtkInteractorStyleRubberBandPick); RectangularFrustumSelector (); - - ~RectangularFrustumSelector (); /** \brief Pass a pointer to the actor map * \param[in] actors the actor map that will be used with this style diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h index cd4352ac..3a9a20b3 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/point_selectors/selected_trackball_interactor_style.h @@ -53,8 +53,6 @@ namespace pcl vtkTypeMacro(SelectedTrackballStyleInteractor,vtkInteractorStyleTrackballActor); SelectedTrackballStyleInteractor (); - - ~SelectedTrackballStyleInteractor (); /** \brief Pass a pointer to the actor map * \param[in] actors the actor map that will be used with this style diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h index 2d365701..57460d14 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/properties_model.h @@ -54,7 +54,6 @@ namespace pcl /** \brief Constructor used for item parameters */ PropertiesModel (CloudComposerItem* parent_item, QObject *parent = nullptr); PropertiesModel (const PropertiesModel& to_copy); - ~PropertiesModel (); /** \brief Helper function for adding a new property */ void diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h index 7ec851a9..1eb18eaf 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tool_interface/abstract_tool.h @@ -95,8 +95,6 @@ namespace pcl : AbstractTool (parameter_model, parent) {} - ~ModifyItemTool () { } - QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override = 0; @@ -119,8 +117,6 @@ namespace pcl : AbstractTool (parameter_model, parent) {} - ~NewItemTool () { } - QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override = 0; @@ -143,8 +139,6 @@ namespace pcl : AbstractTool (parameter_model, parent) {} - ~SplitItemTool () { } - QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override = 0; @@ -167,8 +161,6 @@ namespace pcl : AbstractTool (parameter_model, parent) {} - ~MergeCloudTool () { } - QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override = 0; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h index 14efe55c..9a9e82d7 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/euclidean_clustering.h @@ -49,7 +49,6 @@ namespace pcl Q_OBJECT public: EuclideanClusteringTool (PropertiesModel* parameter_model, QObject* parent); - ~EuclideanClusteringTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h index 10deef32..10287e4c 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/fpfh_estimation.h @@ -50,7 +50,6 @@ namespace pcl Q_OBJECT public: FPFHEstimationTool (PropertiesModel* parameter_model, QObject* parent); - ~FPFHEstimationTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/normal_estimation.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/normal_estimation.h index ae5caee5..b5091cc0 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/normal_estimation.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/normal_estimation.h @@ -49,7 +49,6 @@ namespace pcl Q_OBJECT public: NormalEstimationTool (PropertiesModel* parameter_model, QObject* parent); - ~NormalEstimationTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h index 4541e2e9..018c7a8e 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/organized_segmentation.h @@ -50,7 +50,6 @@ Q_OBJECT public: OrganizedSegmentationTool (PropertiesModel* parameter_model, QObject* parent); - ~OrganizedSegmentationTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h index 936fa9a0..f1c54653 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/sanitize_cloud.h @@ -49,7 +49,6 @@ namespace pcl Q_OBJECT public: SanitizeCloudTool (PropertiesModel* parameter_model, QObject* parent); - ~SanitizeCloudTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h index 7f2114f6..86ce2cbe 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/statistical_outlier_removal.h @@ -50,7 +50,6 @@ namespace pcl Q_OBJECT public: StatisticalOutlierRemovalTool (PropertiesModel* parameter_model, QObject* parent); - ~StatisticalOutlierRemovalTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h index de8bc530..39c6ea66 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/supervoxels.h @@ -50,7 +50,6 @@ Q_OBJECT public: SupervoxelsTool (PropertiesModel* parameter_model, QObject* parent); - ~SupervoxelsTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h index 54b56b20..85499ca8 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/tools/voxel_grid_downsample.h @@ -49,7 +49,6 @@ namespace pcl Q_OBJECT public: VoxelGridDownsampleTool (PropertiesModel* parameter_model, QObject* parent); - ~VoxelGridDownsampleTool (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/transform_clouds.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/transform_clouds.h index 83982be1..e2215296 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/transform_clouds.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/transform_clouds.h @@ -48,7 +48,6 @@ namespace pcl Q_OBJECT public: TransformClouds (QMap > transform_map, QObject* parent = nullptr); - ~TransformClouds (); QList performAction (QList input_data, PointTypeFlags::PointType type = PointTypeFlags::NONE) override; diff --git a/apps/cloud_composer/include/pcl/apps/cloud_composer/work_queue.h b/apps/cloud_composer/include/pcl/apps/cloud_composer/work_queue.h index b91a7db3..95652a67 100644 --- a/apps/cloud_composer/include/pcl/apps/cloud_composer/work_queue.h +++ b/apps/cloud_composer/include/pcl/apps/cloud_composer/work_queue.h @@ -59,7 +59,6 @@ namespace pcl Q_OBJECT public: WorkQueue (QObject* parent = nullptr); - ~WorkQueue(); public Q_SLOTS: void enqueueNewAction (AbstractTool* new_tool, ConstItemList input_data); diff --git a/apps/cloud_composer/src/cloud_browser.cpp b/apps/cloud_composer/src/cloud_browser.cpp index c11a5311..6bdef9b3 100644 --- a/apps/cloud_composer/src/cloud_browser.cpp +++ b/apps/cloud_composer/src/cloud_browser.cpp @@ -28,7 +28,7 @@ pcl::cloud_composer::BackgroundDelegate::paint (QPainter *painter, const QStyleO // if (background.canConvert ()) // painter->fillRect (option.rect, background.value ()); - QVariant text_color_variant = index.data (Qt::TextColorRole); + QVariant text_color_variant = index.data (Qt::ForegroundRole); if (text_color_variant.canConvert ()) { QColor text_color = text_color_variant.value (); diff --git a/apps/cloud_composer/src/cloud_viewer.cpp b/apps/cloud_composer/src/cloud_viewer.cpp index f9c1c385..f47c1c05 100644 --- a/apps/cloud_composer/src/cloud_viewer.cpp +++ b/apps/cloud_composer/src/cloud_viewer.cpp @@ -12,11 +12,6 @@ pcl::cloud_composer::CloudViewer::CloudViewer (QWidget* parent) this, SLOT (modelChanged (int))); } -pcl::cloud_composer::CloudViewer::~CloudViewer () -{ - -} - void pcl::cloud_composer::CloudViewer::addModel (ProjectModel* new_model) { diff --git a/apps/cloud_composer/src/commands.cpp b/apps/cloud_composer/src/commands.cpp index 4b0e5339..c8a0b973 100644 --- a/apps/cloud_composer/src/commands.cpp +++ b/apps/cloud_composer/src/commands.cpp @@ -180,13 +180,12 @@ pcl::cloud_composer::CloudCommand::restoreOriginalRemoveNew (const QList removed = parent_item->takeRow (to_remove_index.row ()); } //Restore the original items - QMap ::iterator itr; foreach (const CloudComposerItem* item, originals) { //Point iterator to the correct spot // Find doesn't modify parameter so it should accept a const pointer, but it can't be because it is templated to the map type // So we hack to get around this with a const cast - itr = removed_to_parent_map_.find (const_cast (item)); + const auto& itr = removed_to_parent_map_.find (const_cast (item)); QStandardItem* parent = itr.value (); QStandardItem* original = itr.key (); parent->appendRow (original); diff --git a/apps/cloud_composer/src/item_inspector.cpp b/apps/cloud_composer/src/item_inspector.cpp index b442d130..c32914f2 100644 --- a/apps/cloud_composer/src/item_inspector.cpp +++ b/apps/cloud_composer/src/item_inspector.cpp @@ -14,11 +14,6 @@ pcl::cloud_composer::ItemInspector::ItemInspector (QWidget* parent) addTab (parameter_view_, "Parameters"); -} - -pcl::cloud_composer::ItemInspector::~ItemInspector () -{ - } void diff --git a/apps/cloud_composer/src/items/cloud_composer_item.cpp b/apps/cloud_composer/src/items/cloud_composer_item.cpp index a975a91b..2d5c786f 100644 --- a/apps/cloud_composer/src/items/cloud_composer_item.cpp +++ b/apps/cloud_composer/src/items/cloud_composer_item.cpp @@ -20,7 +20,6 @@ pcl::cloud_composer::CloudComposerItem::CloudComposerItem (const QString& name) pcl::cloud_composer::CloudComposerItem::~CloudComposerItem () { properties_->deleteLater (); - qDebug () << "Cloud Composer Item Destructor"; } pcl::cloud_composer::CloudComposerItem* diff --git a/apps/cloud_composer/src/items/cloud_item.cpp b/apps/cloud_composer/src/items/cloud_item.cpp index d7b1d83f..8f70e6b0 100644 --- a/apps/cloud_composer/src/items/cloud_item.cpp +++ b/apps/cloud_composer/src/items/cloud_item.cpp @@ -67,12 +67,6 @@ pcl::cloud_composer::CloudItem::clone () const return new_item; } -pcl::cloud_composer::CloudItem::~CloudItem () -{ - qDebug () << "Cloud item destructor"; -} - - void pcl::cloud_composer::CloudItem::paintView (pcl::visualization::PCLVisualizer::Ptr vis) const { diff --git a/apps/cloud_composer/src/items/fpfh_item.cpp b/apps/cloud_composer/src/items/fpfh_item.cpp index f48b5623..96a7e9b3 100644 --- a/apps/cloud_composer/src/items/fpfh_item.cpp +++ b/apps/cloud_composer/src/items/fpfh_item.cpp @@ -27,10 +27,6 @@ pcl::cloud_composer::FPFHItem::clone () const return new_item; } -pcl::cloud_composer::FPFHItem::~FPFHItem () -{ -} - QMap pcl::cloud_composer::FPFHItem::getInspectorTabs () { diff --git a/apps/cloud_composer/src/items/normals_item.cpp b/apps/cloud_composer/src/items/normals_item.cpp index 21e202a5..02267ad7 100644 --- a/apps/cloud_composer/src/items/normals_item.cpp +++ b/apps/cloud_composer/src/items/normals_item.cpp @@ -30,11 +30,6 @@ pcl::cloud_composer::NormalsItem::clone () const return new_item; } -pcl::cloud_composer::NormalsItem::~NormalsItem () -{ - -} - void pcl::cloud_composer::NormalsItem::paintView (pcl::visualization::PCLVisualizer::Ptr vis) const { diff --git a/apps/cloud_composer/src/merge_selection.cpp b/apps/cloud_composer/src/merge_selection.cpp index f8aadeca..63b670f2 100644 --- a/apps/cloud_composer/src/merge_selection.cpp +++ b/apps/cloud_composer/src/merge_selection.cpp @@ -14,11 +14,6 @@ pcl::cloud_composer::MergeSelection::MergeSelection (QMap pcl::cloud_composer::MergeSelection::performAction (ConstItemList input_data, PointTypeFlags::PointType type) { diff --git a/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp b/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp index c21b2125..141c5d3d 100644 --- a/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp +++ b/apps/cloud_composer/src/point_selectors/click_trackball_interactor_style.cpp @@ -24,11 +24,6 @@ pcl::cloud_composer::ClickTrackballStyleInteractor::ClickTrackballStyleInteracto transform_ = vtkSmartPointer::New (); } -pcl::cloud_composer::ClickTrackballStyleInteractor::~ClickTrackballStyleInteractor () -{ - -} - void pcl::cloud_composer::ClickTrackballStyleInteractor::OnLeftButtonDown () { diff --git a/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp b/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp index 72fbf7f6..3744a244 100644 --- a/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp +++ b/apps/cloud_composer/src/point_selectors/interactor_style_switch.cpp @@ -38,11 +38,6 @@ pcl::cloud_composer::InteractorStyleSwitch::InteractorStyleSwitch () } -pcl::cloud_composer::InteractorStyleSwitch::~InteractorStyleSwitch () -{ - -} - void pcl::cloud_composer::InteractorStyleSwitch::initializeInteractorStyles (pcl::visualization::PCLVisualizer::Ptr vis, ProjectModel* model) { diff --git a/apps/cloud_composer/src/point_selectors/manipulation_event.cpp b/apps/cloud_composer/src/point_selectors/manipulation_event.cpp index 7021de8b..7c70d738 100644 --- a/apps/cloud_composer/src/point_selectors/manipulation_event.cpp +++ b/apps/cloud_composer/src/point_selectors/manipulation_event.cpp @@ -1,11 +1,5 @@ #include -pcl::cloud_composer::ManipulationEvent::~ManipulationEvent () -{ - //TODO Delete manipulated actor here? - -} - void pcl::cloud_composer::ManipulationEvent::addManipulation (const QString& id, const vtkSmartPointer& start, const vtkSmartPointer& end) { diff --git a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp index 0bd7114c..22f0de1c 100644 --- a/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp +++ b/apps/cloud_composer/src/point_selectors/rectangular_frustum_selector.cpp @@ -24,12 +24,6 @@ pcl::cloud_composer::RectangularFrustumSelector::RectangularFrustumSelector () selection_complete_event_ = interactor_events::SELECTION_COMPLETE_EVENT; } -pcl::cloud_composer::RectangularFrustumSelector::~RectangularFrustumSelector () -{ - -} - - void pcl::cloud_composer::RectangularFrustumSelector::OnLeftButtonUp () { diff --git a/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp b/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp index 9ad0f579..264e5f2f 100644 --- a/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp +++ b/apps/cloud_composer/src/point_selectors/selected_trackball_interactor_style.cpp @@ -26,11 +26,6 @@ pcl::cloud_composer::SelectedTrackballStyleInteractor::SelectedTrackballStyleInt manipulation_complete_event_ = interactor_events::MANIPULATION_COMPLETE_EVENT; } -pcl::cloud_composer::SelectedTrackballStyleInteractor::~SelectedTrackballStyleInteractor () -{ - -} - void pcl::cloud_composer::SelectedTrackballStyleInteractor::setSelectedActors () { diff --git a/apps/cloud_composer/src/project_model.cpp b/apps/cloud_composer/src/project_model.cpp index 5be855c4..39963959 100644 --- a/apps/cloud_composer/src/project_model.cpp +++ b/apps/cloud_composer/src/project_model.cpp @@ -444,7 +444,7 @@ pcl::cloud_composer::ProjectModel::clearSelection () foreach (CloudItem* selected_item, selected_item_index_map_.keys()) { qDebug () << "Setting item color back to black"; - selected_item->setForeground (QBrush (Qt::black));; + selected_item->setForeground (QBrush (Qt::black)); } selected_item_index_map_.clear (); @@ -583,7 +583,7 @@ pcl::cloud_composer::ProjectModel::itemSelectionChanged ( const QItemSelection & //Set all point selected cloud items back to green text, since if they are selected they get changed to white foreach (CloudItem* selected_item, selected_item_index_map_.keys()) { - selected_item->setForeground (QBrush (Qt::green));; + selected_item->setForeground (QBrush (Qt::green)); } } @@ -610,7 +610,7 @@ pcl::cloud_composer::ProjectModel::onlyCloudItemsSelected () void pcl::cloud_composer::ProjectModel::setSelectedStyle (interactor_styles::INTERACTOR_STYLES style) { - QMap::iterator itr = selected_style_map_.begin(); + auto itr = selected_style_map_.begin(); while (itr != selected_style_map_.end ()) { itr.value() = false; diff --git a/apps/cloud_composer/src/properties_model.cpp b/apps/cloud_composer/src/properties_model.cpp index 7ec4015b..5ac24ddb 100644 --- a/apps/cloud_composer/src/properties_model.cpp +++ b/apps/cloud_composer/src/properties_model.cpp @@ -38,10 +38,6 @@ pcl::cloud_composer::PropertiesModel::PropertiesModel (const PropertiesModel& to } } -pcl::cloud_composer::PropertiesModel::~PropertiesModel () -{ -} - void pcl::cloud_composer::PropertiesModel::addProperty (const QString& prop_name, const QVariant& value, Qt::ItemFlags flags, const QString& category) { diff --git a/apps/cloud_composer/src/toolbox_model.cpp b/apps/cloud_composer/src/toolbox_model.cpp index b6141fac..dd70fbd2 100644 --- a/apps/cloud_composer/src/toolbox_model.cpp +++ b/apps/cloud_composer/src/toolbox_model.cpp @@ -183,7 +183,7 @@ pcl::cloud_composer::ToolBoxModel::updateEnabledTools (const QItemSelection& cur else if ( ! type_items_map.keys ().contains (input_type)) { enabled_itr.remove (); - disabled_tools.insert (tool_item, tr("Tool Requires item type %1 selected").arg (ITEM_TYPES_STRINGS.value (input_type - QStandardItem::UserType))); + disabled_tools.insert (tool_item, tr("Tool Requires item type %1 selected").arg (ITEM_TYPES_STRINGS.value (input_type - CloudComposerItem::CLOUD_COMPOSER_ITEM))); } //Check if any of selected items have required children else if ( !required_children_types.empty ()) @@ -222,7 +222,7 @@ pcl::cloud_composer::ToolBoxModel::updateEnabledTools (const QItemSelection& cur enabled_itr.remove (); QString missing_children_string; foreach (CloudComposerItem::ItemType type, missing_children) - missing_children_string.append (" "+ITEM_TYPES_STRINGS.value (type - QStandardItem::UserType)); + missing_children_string.append (" "+ITEM_TYPES_STRINGS.value (type - CloudComposerItem::CLOUD_COMPOSER_ITEM)); disabled_tools.insert (tool_item, tr ("Tool Requires child item of type(s) %1").arg (missing_children_string)); } } diff --git a/apps/cloud_composer/src/transform_clouds.cpp b/apps/cloud_composer/src/transform_clouds.cpp index 1953fd3e..54658e74 100644 --- a/apps/cloud_composer/src/transform_clouds.cpp +++ b/apps/cloud_composer/src/transform_clouds.cpp @@ -12,11 +12,6 @@ pcl::cloud_composer::TransformClouds::TransformClouds (QMap pcl::cloud_composer::TransformClouds::performAction (ConstItemList input_data, PointTypeFlags::PointType type) { diff --git a/apps/cloud_composer/src/work_queue.cpp b/apps/cloud_composer/src/work_queue.cpp index 82826016..2d4fb11d 100644 --- a/apps/cloud_composer/src/work_queue.cpp +++ b/apps/cloud_composer/src/work_queue.cpp @@ -7,13 +7,6 @@ pcl::cloud_composer::WorkQueue::WorkQueue (QObject* parent) -} - - -pcl::cloud_composer::WorkQueue::~WorkQueue ( ) -{ - - } void diff --git a/apps/cloud_composer/tools/euclidean_clustering.cpp b/apps/cloud_composer/tools/euclidean_clustering.cpp index 13f5d914..fc1a4073 100644 --- a/apps/cloud_composer/tools/euclidean_clustering.cpp +++ b/apps/cloud_composer/tools/euclidean_clustering.cpp @@ -15,11 +15,6 @@ pcl::cloud_composer::EuclideanClusteringTool::EuclideanClusteringTool (Propertie } -pcl::cloud_composer::EuclideanClusteringTool::~EuclideanClusteringTool () -{ - -} - QList pcl::cloud_composer::EuclideanClusteringTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { @@ -74,13 +69,13 @@ pcl::cloud_composer::EuclideanClusteringTool::performAction (ConstItemList input qDebug () << "Found "< filter; - for (std::vector::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) + for (const auto& cluster : cluster_indices) { filter.setInputCloud (input_cloud); // It's annoying that I have to do this, but Euclidean returns a PointIndices struct - pcl::PointIndices::ConstPtr indices_ptr = pcl::make_shared(*it); + pcl::PointIndices::ConstPtr indices_ptr = pcl::make_shared(cluster); filter.setIndices (indices_ptr); - extracted_indices->insert (extracted_indices->end (), it->indices.begin (), it->indices.end ()); + extracted_indices->insert (extracted_indices->end (), cluster.indices.begin (), cluster.indices.end ()); //This means remove the other points filter.setKeepOrganized (false); pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2); diff --git a/apps/cloud_composer/tools/fpfh_estimation.cpp b/apps/cloud_composer/tools/fpfh_estimation.cpp index c46407ea..a65056a1 100644 --- a/apps/cloud_composer/tools/fpfh_estimation.cpp +++ b/apps/cloud_composer/tools/fpfh_estimation.cpp @@ -14,11 +14,6 @@ pcl::cloud_composer::FPFHEstimationTool::FPFHEstimationTool (PropertiesModel* pa { -} - -pcl::cloud_composer::FPFHEstimationTool::~FPFHEstimationTool () -{ - } QList diff --git a/apps/cloud_composer/tools/normal_estimation.cpp b/apps/cloud_composer/tools/normal_estimation.cpp index 4c1e232e..55ced5d3 100644 --- a/apps/cloud_composer/tools/normal_estimation.cpp +++ b/apps/cloud_composer/tools/normal_estimation.cpp @@ -11,11 +11,6 @@ pcl::cloud_composer::NormalEstimationTool::NormalEstimationTool (PropertiesModel { -} - -pcl::cloud_composer::NormalEstimationTool::~NormalEstimationTool () -{ - } QList diff --git a/apps/cloud_composer/tools/organized_segmentation.cpp b/apps/cloud_composer/tools/organized_segmentation.cpp index 2f8330e2..47d7430f 100644 --- a/apps/cloud_composer/tools/organized_segmentation.cpp +++ b/apps/cloud_composer/tools/organized_segmentation.cpp @@ -17,11 +17,6 @@ pcl::cloud_composer::OrganizedSegmentationTool::OrganizedSegmentationTool (Prope } -pcl::cloud_composer::OrganizedSegmentationTool::~OrganizedSegmentationTool () -{ - -} - QList pcl::cloud_composer::OrganizedSegmentationTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) { diff --git a/apps/cloud_composer/tools/sanitize_cloud.cpp b/apps/cloud_composer/tools/sanitize_cloud.cpp index ea22f4ad..d56d5276 100644 --- a/apps/cloud_composer/tools/sanitize_cloud.cpp +++ b/apps/cloud_composer/tools/sanitize_cloud.cpp @@ -10,10 +10,6 @@ pcl::cloud_composer::SanitizeCloudTool::SanitizeCloudTool (PropertiesModel* para { } -pcl::cloud_composer::SanitizeCloudTool::~SanitizeCloudTool () -{ -} - QList pcl::cloud_composer::SanitizeCloudTool::performAction (ConstItemList input_data, PointTypeFlags::PointType) { diff --git a/apps/cloud_composer/tools/statistical_outlier_removal.cpp b/apps/cloud_composer/tools/statistical_outlier_removal.cpp index 95ae8917..3213a975 100644 --- a/apps/cloud_composer/tools/statistical_outlier_removal.cpp +++ b/apps/cloud_composer/tools/statistical_outlier_removal.cpp @@ -11,11 +11,6 @@ pcl::cloud_composer::StatisticalOutlierRemovalTool::StatisticalOutlierRemovalToo { -} - -pcl::cloud_composer::StatisticalOutlierRemovalTool::~StatisticalOutlierRemovalTool () -{ - } QList diff --git a/apps/cloud_composer/tools/supervoxels.cpp b/apps/cloud_composer/tools/supervoxels.cpp index 6b9be7f5..6c22b37f 100644 --- a/apps/cloud_composer/tools/supervoxels.cpp +++ b/apps/cloud_composer/tools/supervoxels.cpp @@ -15,11 +15,6 @@ pcl::cloud_composer::SupervoxelsTool::SupervoxelsTool (PropertiesModel* paramete } -pcl::cloud_composer::SupervoxelsTool::~SupervoxelsTool () -{ - -} - QList pcl::cloud_composer::SupervoxelsTool::performAction (ConstItemList input_data, PointTypeFlags::PointType type) { diff --git a/apps/cloud_composer/tools/voxel_grid_downsample.cpp b/apps/cloud_composer/tools/voxel_grid_downsample.cpp index d6efb21c..5b15c340 100644 --- a/apps/cloud_composer/tools/voxel_grid_downsample.cpp +++ b/apps/cloud_composer/tools/voxel_grid_downsample.cpp @@ -12,11 +12,6 @@ pcl::cloud_composer::VoxelGridDownsampleTool::VoxelGridDownsampleTool (Propertie { -} - -pcl::cloud_composer::VoxelGridDownsampleTool::~VoxelGridDownsampleTool () -{ - } QList diff --git a/apps/in_hand_scanner/CMakeLists.txt b/apps/in_hand_scanner/CMakeLists.txt index 2096cbed..24d56ba6 100644 --- a/apps/in_hand_scanner/CMakeLists.txt +++ b/apps/in_hand_scanner/CMakeLists.txt @@ -11,8 +11,8 @@ if(${DEFAULT} STREQUAL "TRUE") set(DEFAULT FALSE) endif() -pcl_subsubsys_option(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") -pcl_subsubsys_depend(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) +pcl_subsubsys_option(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSYS_DEPEND(build NAME ${SUBSUBSYS_NAME} PARENT_NAME ${SUBSYS_NAME} DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) pcl_add_doc("${SUBSUBSYS_NAME}") diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h index 68491e6f..f4f53542 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/boost.h @@ -41,7 +41,7 @@ #pragma once #ifdef __GNUC__ -# pragma GCC system_header +#pragma GCC system_header #endif PCL_DEPRECATED_HEADER(1, 15, "Please include the needed boost headers directly.") #include diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/common_types.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/common_types.h index f963aa9a..d5c32cea 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/common_types.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/common_types.h @@ -40,55 +40,52 @@ #pragma once -#include - +#include #include #include -#include -namespace pcl -{ - namespace ihs - { - struct PointIHS; - using CloudIHS = pcl::PointCloud; - using CloudIHSPtr = CloudIHS::Ptr; - using CloudIHSConstPtr = CloudIHS::ConstPtr; - } // End namespace ihs +#include + +namespace pcl { +namespace ihs { +struct PointIHS; +using CloudIHS = pcl::PointCloud; +using CloudIHSPtr = CloudIHS::Ptr; +using CloudIHSConstPtr = CloudIHS::ConstPtr; +} // End namespace ihs } // End namespace pcl #include -POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::ihs::_PointIHS, - (float, x, x) - (float, y, y) - (float, z, z) - (float, normal_x, normal_x) - (float, normal_y, normal_y) - (float, normal_z, normal_z) - (float, rgb, rgb) - (float, weight, weight) - (unsigned int, age, age) - (std::uint32_t, directions, directions) - ) -POINT_CLOUD_REGISTER_POINT_WRAPPER (pcl::ihs::PointIHS, pcl::ihs::_PointIHS) +// clang-format off +POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::ihs::_PointIHS, + (float, x, x) + (float, y, y) + (float, z, z) + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, rgb, rgb) + (float, weight, weight) + (unsigned int, age, age) + (std::uint32_t, directions, directions) + ) +POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::ihs::PointIHS, pcl::ihs::_PointIHS) +// clang-format on -namespace pcl -{ - namespace ihs - { - struct MeshTraits - { - using VertexData = PointIHS; - using HalfEdgeData = pcl::geometry::NoData; - using EdgeData = pcl::geometry::NoData; - using FaceData = pcl::geometry::NoData; - using IsManifold = std::true_type; - }; +namespace pcl { +namespace ihs { +struct MeshTraits { + using VertexData = PointIHS; + using HalfEdgeData = pcl::geometry::NoData; + using EdgeData = pcl::geometry::NoData; + using FaceData = pcl::geometry::NoData; + using IsManifold = std::true_type; +}; - // NOTE: The drawMesh method in pcl::ihs::InHandScanner only supports triangles! - using Mesh = pcl::geometry::TriangleMesh; - using MeshPtr = Mesh::Ptr; - using MeshConstPtr = Mesh::ConstPtr; - } // End namespace ihs +// NOTE: The drawMesh method in pcl::ihs::InHandScanner only supports triangles! +using Mesh = pcl::geometry::TriangleMesh; +using MeshPtr = Mesh::Ptr; +using MeshConstPtr = Mesh::ConstPtr; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h index ccbefab4..a4380316 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/eigen.h @@ -41,9 +41,9 @@ #pragma once #ifdef __GNUC__ -# pragma GCC system_header +#pragma GCC system_header #endif PCL_DEPRECATED_HEADER(1, 15, "Please include the needed eigen headers directly.") +#include #include #include -#include diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/help_window.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/help_window.h index abfdbd89..acac2e55 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/help_window.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/help_window.h @@ -42,25 +42,21 @@ #include -namespace Ui -{ - class HelpWindow; +namespace Ui { +class HelpWindow; } -namespace pcl -{ - namespace ihs - { - class HelpWindow : public QDialog - { - Q_OBJECT +namespace pcl { +namespace ihs { +class HelpWindow : public QDialog { + Q_OBJECT - public: - explicit HelpWindow (QWidget* parent = nullptr); - ~HelpWindow (); +public: + explicit HelpWindow(QWidget* parent = nullptr); + ~HelpWindow() override; - private: - Ui::HelpWindow* ui; - }; - } // End namespace ihs +private: + Ui::HelpWindow* ui; +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h index 1448564f..7496c578 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/icp.h @@ -40,172 +40,200 @@ #pragma once -#include #include #include +#include //////////////////////////////////////////////////////////////////////////////// // ICP //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - /** \brief Iterative Closest Point registration. - * \author Martin Saelzle - * \ingroup apps - */ - class PCL_EXPORTS ICP - { - public: - - using PointXYZRGBNormal = pcl::PointXYZRGBNormal; - using CloudXYZRGBNormal = pcl::PointCloud; - using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; - using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - - using Mesh = pcl::ihs::Mesh; - using MeshPtr = pcl::ihs::MeshPtr; - using MeshConstPtr = pcl::ihs::MeshConstPtr; - - /** \brief Constructor */ - ICP (); - - /** @{ */ - /** \brief Convergence is detected when the change of the fitness between the current and previous iteration becomes smaller than the given epsilon (set in cm^2). The fitness is the mean squared euclidean distance between corresponding points. - * \note Only accepted if it is greater than 0. - */ - void - setEpsilon (const float epsilon); - - float - getEpsilon () const; - /** @} */ - - /** @{ */ - /** \brief The registration fails if the number of iterations exceeds the maximum number of iterations. - * \note Must be greater than 0. Smaller values are set to 1. - */ - void - setMaxIterations (const unsigned int max_iter); - - unsigned int - getMaxIterations () const; - /** @} */ - - /** @{ */ - /** \brief The registration fails at the state of convergence if the overlap between the model and data shape is smaller than a minimum overlap. The overlap is the fraction of correspondences (after rejection) to the initial number of data points. - * \note Must be between zero and one. Values outside this range are clamped to the nearest valid value. - */ - void - setMinOverlap (const float overlap); - - float - getMinOverlap () const; - /** @} */ - - /** @{ */ - /** \brief The registration fails at the state of convergence if the fitness is bigger than this threshold (set in cm^2) - * \note Must be greater than zero. - */ - void - setMaxFitness (const float fitness); - - float - getMaxFitness () const; - /** @} */ - - /** @{ */ - /** \brief Correspondences are rejected if the squared distance is above a threshold. This threshold is initialized with infinity (all correspondences are accepted in the first iteration). The threshold of the next iterations is set to the fitness of the current iteration multiplied by the factor set by this method. - * \note Must be greater or equal one. Smaller values are set to one. - */ - void - setCorrespondenceRejectionFactor (const float factor); - - float - getCorrespondenceRejectionFactor () const; - /** @} */ - - /** @{ */ - /** \brief Correspondences are rejected if the angle between the normals is bigger than this threshold. Set in degrees. - * \note Must be between 180 degrees and 0. Values outside this range are clamped to the nearest valid value. - */ - void - setMaxAngle (const float angle); - - float - getMaxAngle () const; - /** @} */ - - /** \brief Find the transformation that aligns the data cloud (source) to the model mesh (target). - * \param[in] mesh_model Model mesh (target). - * \param[in] cloud_data Data cloud (source). - * \param[in] T_init Initial guess for the transformation. - * \paran[out] T_final The computed transformation. - * \return true if success. - */ - bool - findTransformation (const MeshConstPtr& mesh_model, - const CloudXYZRGBNormalConstPtr& cloud_data, - const Eigen::Matrix4f& T_init, - Eigen::Matrix4f& T_final); - - private: - - using PointNormal = pcl::PointNormal; - using CloudNormal = pcl::PointCloud; - using CloudNormalPtr = CloudNormal::Ptr; - using CloudNormalConstPtr = CloudNormal::ConstPtr; - - using KdTree = pcl::KdTree; - using KdTreePtr = KdTree::Ptr; - using KdTreeConstPtr = KdTree::ConstPtr; - - /** \brief Selects the model points that are pointing towards to the camera (data coordinate system = camera coordinate system). - * \param[in] mesh_model Input mesh. - * \param[in] T_inv Transformation that brings the model mesh into the camera coordinate system. - * \return Cloud containing the selected points (the connectivity information of the mesh is currently not used during the registration). - */ - CloudNormalConstPtr - selectModelPoints (const MeshConstPtr& mesh_model, - const Eigen::Matrix4f& T_inv) const; - - /** \brief Selects the valid data points. The input cloud is organized -> contains nans which are removed - * \param[in] cloud_data Input cloud. - * \return Cloud containing the selected points. - */ - CloudNormalConstPtr - selectDataPoints (const CloudXYZRGBNormalConstPtr& cloud_data) const; - - /** \brief Finds the transformation that minimizes the point to plane distance from the source to the target cloud. The input clouds must be arranged to have one to one correspondences (point 0 in source corresponds to point 0 in target, point 1 in source to point 1 in target and so on). - * \param[in] cloud_source Source cloud (data). - * \param[in] cloud_target Target cloud (model). - * \param[out] T The computed transformation. - * \return true if success - */ - bool - minimizePointPlane (const CloudNormal& cloud_source, - const CloudNormal& cloud_target, - Eigen::Matrix4f& T) const; - - //////////////////////////////////////////////////////////////////////// - // Members - //////////////////////////////////////////////////////////////////////// - - KdTreePtr kd_tree_; - - // Convergence - float epsilon_; // in cm^2 - - // Registration failure - unsigned int max_iterations_; - float min_overlap_; // [0 1] - float max_fitness_; // in cm^2 - - // Correspondence rejection - float factor_; - float max_angle_; // in degrees - }; - } // End namespace ihs +namespace pcl { +namespace ihs { +/** \brief Iterative Closest Point registration. + * \author Martin Saelzle + * \ingroup apps + */ +class PCL_EXPORTS ICP { +public: + using PointXYZRGBNormal = pcl::PointXYZRGBNormal; + using CloudXYZRGBNormal = pcl::PointCloud; + using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; + using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; + + using Mesh = pcl::ihs::Mesh; + using MeshPtr = pcl::ihs::MeshPtr; + using MeshConstPtr = pcl::ihs::MeshConstPtr; + + /** \brief Constructor */ + ICP(); + + /** @{ */ + /** \brief Convergence is detected when the change of the fitness between the current + * and previous iteration becomes smaller than the given epsilon (set in cm^2). The + * fitness is the mean squared euclidean distance between corresponding points. + * + * \note Only accepted if it is greater than 0. + */ + void + setEpsilon(const float epsilon); + + float + getEpsilon() const; + /** @} */ + + /** @{ */ + /** \brief The registration fails if the number of iterations exceeds the maximum + * number of iterations. + * + * \note Must be greater than 0. Smaller values are set to 1. + */ + void + setMaxIterations(const unsigned int max_iter); + + unsigned int + getMaxIterations() const; + /** @} */ + + /** @{ */ + /** \brief The registration fails at the state of convergence if the overlap between + * the model and data shape is smaller than a minimum overlap. The overlap is the + * fraction of correspondences (after rejection) to the initial number of data points. + * + * \note Must be between zero and one. Values outside this range are clamped to the + * nearest valid value. + */ + void + setMinOverlap(const float overlap); + + float + getMinOverlap() const; + /** @} */ + + /** @{ */ + /** \brief The registration fails at the state of convergence if the fitness is bigger + * than this threshold (set in cm^2) + * + * \note Must be greater than zero. + */ + void + setMaxFitness(const float fitness); + + float + getMaxFitness() const; + /** @} */ + + /** @{ */ + /** \brief Correspondences are rejected if the squared distance is above a threshold. + * This threshold is initialized with infinity (all correspondences are accepted in + * the first iteration). The threshold of the next iterations is set to the fitness of + * the current iteration multiplied by the factor set by this method. + * + * \note Must be greater or equal one. Smaller values are set to one. + */ + void + setCorrespondenceRejectionFactor(const float factor); + + float + getCorrespondenceRejectionFactor() const; + /** @} */ + + /** @{ */ + /** \brief Correspondences are rejected if the angle between the normals is bigger + * than this threshold. Set in degrees. + * + * \note Must be between 180 degrees and 0. Values outside this range are clamped to + * the nearest valid value. + */ + void + setMaxAngle(const float angle); + + float + getMaxAngle() const; + /** @} */ + + /** \brief Find the transformation that aligns the data cloud (source) to the model + * mesh (target). + * + * \param[in] mesh_model Model mesh (target). + * \param[in] cloud_data Data cloud (source). + * \param[in] T_init Initial guess for the transformation. + * \param[out] T_final The computed transformation. + * + * \return true if success. + */ + bool + findTransformation(const MeshConstPtr& mesh_model, + const CloudXYZRGBNormalConstPtr& cloud_data, + const Eigen::Matrix4f& T_init, + Eigen::Matrix4f& T_final); + +private: + using PointNormal = pcl::PointNormal; + using CloudNormal = pcl::PointCloud; + using CloudNormalPtr = CloudNormal::Ptr; + using CloudNormalConstPtr = CloudNormal::ConstPtr; + + using KdTree = pcl::KdTree; + using KdTreePtr = KdTree::Ptr; + using KdTreeConstPtr = KdTree::ConstPtr; + + /** \brief Selects the model points that are pointing towards to the camera (data + * coordinate system = camera coordinate system). + * + * \param[in] mesh_model Input mesh. + * \param[in] T_inv Transformation that brings the model mesh into the camera + * coordinate system. + * + * \return Cloud containing the selected points (the connectivity + * information of the mesh is currently not used during the registration). + */ + CloudNormalConstPtr + selectModelPoints(const MeshConstPtr& mesh_model, const Eigen::Matrix4f& T_inv) const; + + /** \brief Selects the valid data points. The input cloud is organized -> contains + * nans which are removed + * + * \param[in] cloud_data Input cloud. + * + * \return Cloud containing the selected points. + */ + CloudNormalConstPtr + selectDataPoints(const CloudXYZRGBNormalConstPtr& cloud_data) const; + + /** \brief Finds the transformation that minimizes the point to plane distance from + * the source to the target cloud. The input clouds must be arranged to have one to + * one correspondences (point 0 in source corresponds to point 0 in target, point 1 in + * source to point 1 in target and so on). + * + * \param[in] cloud_source Source cloud (data). + * \param[in] cloud_target Target cloud (model). + * \param[out] T The computed transformation. + * + * \return true if success + */ + bool + minimizePointPlane(const CloudNormal& cloud_source, + const CloudNormal& cloud_target, + Eigen::Matrix4f& T) const; + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + KdTreePtr kd_tree_; + + // Convergence + float epsilon_; // in cm^2 + + // Registration failure + unsigned int max_iterations_; + float min_overlap_; // [0 1] + float max_fitness_; // in cm^2 + + // Correspondence rejection + float factor_; + float max_angle_; // in degrees +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp index a0e5a1eb..17900bf5 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/impl/common_types.hpp @@ -41,92 +41,98 @@ #ifndef PCL_APPS_IN_HAND_SCANNER_IMPL_COMMON_TYPES_HPP #define PCL_APPS_IN_HAND_SCANNER_IMPL_COMMON_TYPES_HPP -#include - #include #include -namespace pcl -{ - namespace ihs +#include + +namespace pcl { +namespace ihs { +struct EIGEN_ALIGN16 _PointIHS { + PCL_ADD_POINT4D + PCL_ADD_NORMAL4D + PCL_ADD_RGB + float weight; + unsigned int age; + std::uint32_t directions; + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct PointIHS : public pcl::ihs::_PointIHS { + // NOTE: I rely on NaN in the default constructor! + inline PointIHS() + { + this->x = this->y = this->z = std::numeric_limits::quiet_NaN(); + this->data[3] = 1.f; + + this->normal_x = this->normal_y = this->normal_z = + std::numeric_limits::quiet_NaN(); + this->data_n[3] = 0.f; + + this->b = this->g = this->r = 0; + this->a = 255; + + this->weight = 0.f; + this->age = 0; + this->directions = 0; + } + + inline PointIHS(const PointIHS& other) { - struct EIGEN_ALIGN16 _PointIHS - { - PCL_ADD_POINT4D - PCL_ADD_NORMAL4D - PCL_ADD_RGB - float weight; - unsigned int age; - std::uint32_t directions; - - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - - struct PointIHS : public pcl::ihs::_PointIHS - { - // NOTE: I rely on NaN in the default constructor! - inline PointIHS () - { - this->x = this->y = this->z = std::numeric_limits::quiet_NaN (); - this->data[3] = 1.f; - - this->normal_x = this->normal_y = this->normal_z = std::numeric_limits::quiet_NaN (); - this->data_n[3] = 0.f; - - this->b = this->g = this->r = 0; this->a = 255; - - this->weight = 0.f; - this->age = 0; - this->directions = 0; - } - - inline PointIHS (const PointIHS& other) - { - this->x = other.x; - this->y = other.y; - this->z = other.z; - this->data[3] = other.data[3]; - - this->normal_x = other.normal_x; - this->normal_y = other.normal_y; - this->normal_z = other.normal_z; - this->data_n[3] = other.data_n[3]; - - this->rgba = other.rgba; - - this->weight = other.weight; - this->age = other.age; - this->directions = other.directions; - } - - inline PointIHS& operator=(const PointIHS& other) = default; - - inline PointIHS (const pcl::PointXYZRGBNormal& other, const float weight) - { - this->x = other.x; - this->y = other.y; - this->z = other.z; - this->data[3] = other.data[3]; - - this->normal_x = other.normal_x; - this->normal_y = other.normal_y; - this->normal_z = other.normal_z; - this->data_n[3] = other.data_n[3]; - - this->rgba = other.rgba; - - this->weight = weight; - this->age = 0; - this->directions = 0; - } - - // inline Eigen::Vector3i getRGBVector3i () {return (Eigen::Vector3i (r, g, b));} - inline const Eigen::Vector3i getRGBVector3i () const {return (Eigen::Vector3i (r, g, b));} - // inline Eigen::Vector4i getRGBVector4i () {return (Eigen::Vector4i (r, g, b, a));} - inline const Eigen::Vector4i getRGBVector4i () const {return (Eigen::Vector4i (r, g, b, a));} - }; - - } // End namespace ihs + this->x = other.x; + this->y = other.y; + this->z = other.z; + this->data[3] = other.data[3]; + + this->normal_x = other.normal_x; + this->normal_y = other.normal_y; + this->normal_z = other.normal_z; + this->data_n[3] = other.data_n[3]; + + this->rgba = other.rgba; + + this->weight = other.weight; + this->age = other.age; + this->directions = other.directions; + } + + inline PointIHS& + operator=(const PointIHS& other) = default; + + inline PointIHS(const pcl::PointXYZRGBNormal& other, const float weight) + { + this->x = other.x; + this->y = other.y; + this->z = other.z; + this->data[3] = other.data[3]; + + this->normal_x = other.normal_x; + this->normal_y = other.normal_y; + this->normal_z = other.normal_z; + this->data_n[3] = other.data_n[3]; + + this->rgba = other.rgba; + + this->weight = weight; + this->age = 0; + this->directions = 0; + } + + inline const Eigen::Vector3i + getRGBVector3i() const + { + return (Eigen::Vector3i(r, g, b)); + } + + inline const Eigen::Vector4i + getRGBVector4i() const + { + return (Eigen::Vector4i(r, g, b, a)); + } +}; + +} // End namespace ihs } // End namespace pcl #endif // PCL_APPS_IN_HAND_SCANNER_IMPL_COMMON_TYPES_HPP diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h index fa2c81db..389bc815 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/in_hand_scanner.h @@ -40,270 +40,278 @@ #pragma once +#include +#include #include #include #include -#include -#include + #include // for connection + +#include #include -#include #include -#include +#include //////////////////////////////////////////////////////////////////////////////// // Forward declarations //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - class OpenNIGrabber; +namespace pcl { +class OpenNIGrabber; - namespace ihs - { - class ICP; - class InputDataProcessing; - class Integration; - class MeshProcessing; - } // End namespace ihs +namespace ihs { +class ICP; +class InputDataProcessing; +class Integration; +class MeshProcessing; +} // End namespace ihs } // End namespace pcl //////////////////////////////////////////////////////////////////////////////// // InHandScanner //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs +namespace pcl { +namespace ihs { +/** \brief + * \todo Add Documentation + */ +class PCL_EXPORTS InHandScanner : public pcl::ihs::OpenGLViewer { + Q_OBJECT + +public: + using Base = pcl::ihs::OpenGLViewer; + using Self = pcl::ihs::InHandScanner; + + using InputDataProcessing = pcl::ihs::InputDataProcessing; + using InputDataProcessingPtr = std::shared_ptr; + using InputDataProcessingConstPtr = std::shared_ptr; + + using ICP = pcl::ihs::ICP; + using ICPPtr = std::shared_ptr; + using ICPConstPtr = std::shared_ptr; + + using Integration = pcl::ihs::Integration; + using IntegrationPtr = std::shared_ptr; + using IntegrationConstPtr = std::shared_ptr; + + using MeshProcessing = pcl::ihs::MeshProcessing; + using MeshProcessingPtr = std::shared_ptr; + using MeshProcessingConstPtr = std::shared_ptr; + + /** \brief Switch between different branches of the scanning pipeline. */ + enum RunningMode { + RM_SHOW_MODEL = 0, /**< Shows the model shape (if it is available). */ + RM_UNPROCESSED = 1, /**< Shows the unprocessed input data. */ + RM_PROCESSED = 2, /**< Shows the processed input data. */ + RM_REGISTRATION_CONT = + 3, /**< Registers new data to the first acquired data continuously. */ + RM_REGISTRATION_SINGLE = + 4 /**< Registers new data once and returns to showing the processed data. */ + }; + + /** \brief File type for saving and loading files. */ + enum FileType { + FT_PLY = 0, /**< Polygon File Format. */ + FT_VTK = 1 /**< VTK File Format. */ + }; + + /** \brief Constructor. */ + explicit InHandScanner(Base* parent = nullptr); + + /** \brief Destructor. */ + ~InHandScanner() override; + + /** \brief Get the input data processing. */ + inline InputDataProcessing& + getInputDataProcessing() { - /** \brief - * \todo Add Documentation - */ - class PCL_EXPORTS InHandScanner : public pcl::ihs::OpenGLViewer - { - Q_OBJECT - - public: - - using Base = pcl::ihs::OpenGLViewer; - using Self = pcl::ihs::InHandScanner; - - using InputDataProcessing = pcl::ihs::InputDataProcessing; - using InputDataProcessingPtr = std::shared_ptr; - using InputDataProcessingConstPtr = std::shared_ptr; - - using ICP = pcl::ihs::ICP; - using ICPPtr = std::shared_ptr; - using ICPConstPtr = std::shared_ptr; - - using Integration = pcl::ihs::Integration; - using IntegrationPtr = std::shared_ptr; - using IntegrationConstPtr = std::shared_ptr; - - using MeshProcessing = pcl::ihs::MeshProcessing; - using MeshProcessingPtr = std::shared_ptr; - using MeshProcessingConstPtr = std::shared_ptr; - - /** \brief Switch between different branches of the scanning pipeline. */ - enum RunningMode - { - RM_SHOW_MODEL = 0, /**< Shows the model shape (if it is available). */ - RM_UNPROCESSED = 1, /**< Shows the unprocessed input data. */ - RM_PROCESSED = 2, /**< Shows the processed input data. */ - RM_REGISTRATION_CONT = 3, /**< Registers new data to the first acquired data continuously. */ - RM_REGISTRATION_SINGLE = 4 /**< Registers new data once and returns to showing the processed data. */ - }; - - /** \brief File type for saving and loading files. */ - enum FileType - { - FT_PLY = 0, /**< Polygon File Format. */ - FT_VTK = 1 /**< VTK File Format. */ - }; - - /** \brief Constructor. */ - explicit InHandScanner (Base* parent=nullptr); - - /** \brief Destructor. */ - ~InHandScanner (); - - /** \brief Get the input data processing. */ - inline InputDataProcessing& - getInputDataProcessing () {return (*input_data_processing_);} - - /** \brief Get the registration. */ - inline ICP& - getICP () {return (*icp_);} - - /** \brief Get the integration. */ - inline Integration& - getIntegration () {return (*integration_);} - - Q_SIGNALS: - - /** \brief Emitted when the running mode changes. */ - void - runningModeChanged (RunningMode new_running_mode) const; - - public Q_SLOTS: - - /** \brief Start the grabber (enables the scanning pipeline). */ - void - startGrabber (); - - /** \brief Shows the unprocessed input data. */ - void - showUnprocessedData (); - - /** \brief Shows the processed input data. */ - void - showProcessedData (); - - /** \brief Registers new data to the first acquired data continuously. */ - void - registerContinuously (); - - /** \brief Registers new data once and returns to showing the processed data. */ - void - registerOnce (); - - /** \brief Show the model shape (if one is available). */ - void - showModel (); - - /** \brief Removes unfit vertices regardless of their age. Unfit vertices are those that have not been observed from enough directions. */ - void - removeUnfitVertices (); - - /** \brief Reset the scanning pipeline. */ - void - reset (); - - /** \brief Saves the model mesh in a file with the given filename and filetype. - * \note The extension of the filename is ignored! - */ - void - saveAs (const std::string& filename, const FileType& filetype); - - /** \see http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent */ - void - keyPressEvent (QKeyEvent* event) override; - - private: - - using PointXYZRGBA = pcl::PointXYZRGBA; - using CloudXYZRGBA = pcl::PointCloud; - using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr; - using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr; - - using PointXYZRGBNormal = pcl::PointXYZRGBNormal; - using CloudXYZRGBNormal = pcl::PointCloud; - using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; - using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - - using PointIHS = pcl::ihs::PointIHS; - using CloudIHS = pcl::ihs::CloudIHS; - using CloudIHSPtr = pcl::ihs::CloudIHSPtr; - using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; - - using Mesh = pcl::ihs::Mesh; - using MeshPtr = pcl::ihs::MeshPtr; - using MeshConstPtr = pcl::ihs::MeshConstPtr; - - using Grabber = pcl::OpenNIGrabber; - using GrabberPtr = std::shared_ptr; - using GrabberConstPtr = std::shared_ptr; - - /** \brief Helper object for the computation thread. Please have a look at the documentation of calcFPS. */ - class ComputationFPS : public Base::FPS - { - public: - ComputationFPS () {} - ~ComputationFPS () {} - }; - - /** \brief Helper object for the visualization thread. Please have a look at the documentation of calcFPS. */ - class VisualizationFPS : public Base::FPS - { - public: - VisualizationFPS () {} - ~VisualizationFPS () {} - }; + return (*input_data_processing_); + } - /** \brief Called when new data arries from the grabber. The grabbing - registration - integration pipeline is implemented here. */ - void - newDataCallback (const CloudXYZRGBAConstPtr& cloud_in); - - /** \see http://doc.qt.digia.com/qt/qwidget.html#paintEvent - * \see http://doc.qt.digia.com/qt/opengl-overpainting.html - */ - void - paintEvent (QPaintEvent* event) override; - - /** \brief Draw text over the opengl scene. - * \see http://doc.qt.digia.com/qt/opengl-overpainting.html - */ - void - drawText (); - - /** \brief Actual implementeation of startGrabber (needed so it can be run in a different thread and doesn't block the application when starting up). */ - void - startGrabberImpl (); - - //////////////////////////////////////////////////////////////////////// - // Members - //////////////////////////////////////////////////////////////////////// - - /** \brief Synchronization. */ - std::mutex mutex_; - - /** \brief Please have a look at the documentation of ComputationFPS. */ - ComputationFPS computation_fps_; - - /** \brief Please have a look at the documentation of VisualizationFPS. */ - VisualizationFPS visualization_fps_; - - /** \brief Switch between different branches of the scanning pipeline. */ - RunningMode running_mode_; - - /** \brief The iteration of the scanning pipeline (grab - register - integrate). */ - unsigned int iteration_; - - /** \brief Used to get new data from the sensor. */ - GrabberPtr grabber_; - - /** \brief This variable is true if the grabber is starting. */ - bool starting_grabber_; - - /** \brief Connection of the grabber signal with the data processing thread. */ - boost::signals2::connection new_data_connection_; - - /** \brief Processes the data from the sensor. Output is input to the registration. */ - InputDataProcessingPtr input_data_processing_; - - /** \brief Registration (Iterative Closest Point). */ - ICPPtr icp_; - - /** \brief Transformation that brings the data cloud into model coordinates. */ - Eigen::Matrix4f transformation_; - - /** \brief Integrate the data cloud into a common model. */ - IntegrationPtr integration_; + /** \brief Get the registration. */ + inline ICP& + getICP() + { + return (*icp_); + } - /** \brief Methods called after the integration. */ - MeshProcessingPtr mesh_processing_; + /** \brief Get the integration. */ + inline Integration& + getIntegration() + { + return (*integration_); + } + +Q_SIGNALS: + + /** \brief Emitted when the running mode changes. */ + void + runningModeChanged(RunningMode new_running_mode) const; + +public Q_SLOTS: + + /** \brief Start the grabber (enables the scanning pipeline). */ + void + startGrabber(); + + /** \brief Shows the unprocessed input data. */ + void + showUnprocessedData(); + + /** \brief Shows the processed input data. */ + void + showProcessedData(); + + /** \brief Registers new data to the first acquired data continuously. */ + void + registerContinuously(); + + /** \brief Registers new data once and returns to showing the processed data. */ + void + registerOnce(); + + /** \brief Show the model shape (if one is available). */ + void + showModel(); + + /** \brief Removes unfit vertices regardless of their age. Unfit vertices are those + * that have not been observed from enough directions. */ + void + removeUnfitVertices(); + + /** \brief Reset the scanning pipeline. */ + void + reset(); + + /** \brief Saves the model mesh in a file with the given filename and filetype. + * + * \note The extension of the filename is ignored! + */ + void + saveAs(const std::string& filename, const FileType& filetype); + + /** \see http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent */ + void + keyPressEvent(QKeyEvent* event) override; + +private: + using PointXYZRGBA = pcl::PointXYZRGBA; + using CloudXYZRGBA = pcl::PointCloud; + using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr; + using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr; + + using PointXYZRGBNormal = pcl::PointXYZRGBNormal; + using CloudXYZRGBNormal = pcl::PointCloud; + using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; + using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - /** \brief Model to which new data is registered to (stored as a mesh). */ - MeshPtr mesh_model_; + using PointIHS = pcl::ihs::PointIHS; + using CloudIHS = pcl::ihs::CloudIHS; + using CloudIHSPtr = pcl::ihs::CloudIHSPtr; + using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; + + using Mesh = pcl::ihs::Mesh; + using MeshPtr = pcl::ihs::MeshPtr; + using MeshConstPtr = pcl::ihs::MeshConstPtr; + + using Grabber = pcl::OpenNIGrabber; + using GrabberPtr = std::shared_ptr; + using GrabberConstPtr = std::shared_ptr; + + /** \brief Helper object for the computation thread. Please have a look at the + * documentation of calcFPS. */ + class ComputationFPS : public Base::FPS { + public: + ComputationFPS() = default; + ~ComputationFPS() = default; + }; + + /** \brief Helper object for the visualization thread. Please have a look at the + * documentation of calcFPS. */ + class VisualizationFPS : public Base::FPS { + public: + VisualizationFPS() = default; + ~VisualizationFPS() = default; + }; + + /** \brief Called when new data arries from the grabber. The grabbing - registration - + * integration pipeline is implemented here. */ + void + newDataCallback(const CloudXYZRGBAConstPtr& cloud_in); + + /** \see http://doc.qt.digia.com/qt/qwidget.html#paintEvent + * \see http://doc.qt.digia.com/qt/opengl-overpainting.html + */ + void + paintEvent(QPaintEvent* event) override; + + /** \brief Draw text over the opengl scene. + * \see http://doc.qt.digia.com/qt/opengl-overpainting.html + */ + void + drawText(); + + /** \brief Actual implementeation of startGrabber (needed so it can be run in a + * different thread and doesn't block the application when starting up). */ + void + startGrabberImpl(); + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + /** \brief Synchronization. */ + std::mutex mutex_; + + /** \brief Please have a look at the documentation of ComputationFPS. */ + ComputationFPS computation_fps_; + + /** \brief Please have a look at the documentation of VisualizationFPS. */ + VisualizationFPS visualization_fps_; + + /** \brief Switch between different branches of the scanning pipeline. */ + RunningMode running_mode_; + + /** \brief The iteration of the scanning pipeline (grab - register - integrate). */ + unsigned int iteration_; + + /** \brief Used to get new data from the sensor. */ + GrabberPtr grabber_; + + /** \brief This variable is true if the grabber is starting. */ + bool starting_grabber_; + + /** \brief Connection of the grabber signal with the data processing thread. */ + boost::signals2::connection new_data_connection_; + + /** \brief Processes the data from the sensor. Output is input to the registration. */ + InputDataProcessingPtr input_data_processing_; + + /** \brief Registration (Iterative Closest Point). */ + ICPPtr icp_; + + /** \brief Transformation that brings the data cloud into model coordinates. */ + Eigen::Matrix4f transformation_; + + /** \brief Integrate the data cloud into a common model. */ + IntegrationPtr integration_; + + /** \brief Methods called after the integration. */ + MeshProcessingPtr mesh_processing_; - /** \brief Prevent the application to crash while closing. */ - bool destructor_called_; + /** \brief Model to which new data is registered to (stored as a mesh). */ + MeshPtr mesh_model_; + + /** \brief Prevent the application to crash while closing. */ + bool destructor_called_; - public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - } // End namespace ihs +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // End namespace ihs } // End namespace pcl // http://doc.qt.digia.com/qt/qmetatype.html#Q_DECLARE_METATYPE -Q_DECLARE_METATYPE (pcl::ihs::InHandScanner::RunningMode) +Q_DECLARE_METATYPE(pcl::ihs::InHandScanner::RunningMode) diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h index 46362e9f..6c238fde 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/input_data_processing.h @@ -40,191 +40,335 @@ #pragma once -#include #include #include #include +#include //////////////////////////////////////////////////////////////////////////////// // InputDataProcessing //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - /** \brief Bundles methods that are applied to the input data from the sensor. - * \author Martin Saelzle - * \ingroup apps - */ - class PCL_EXPORTS InputDataProcessing - { - public: - - using PointXYZRGBA = pcl::PointXYZRGBA; - using CloudXYZRGBA = pcl::PointCloud; - using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr; - using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr; - - using PointXYZRGBNormal = pcl::PointXYZRGBNormal; - using CloudXYZRGBNormal = pcl::PointCloud; - using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; - using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - - /** \brief Constructor */ - InputDataProcessing (); - - /** \brief Apply the segmentation on the input cloud (XYZ and HSV). - * \param[in] cloud_in The input cloud. - * \param[out] cloud_out The segmented cloud. - * \param[out] cloud_discarded Cloud containing all points that were removed during the HSV segmentation. The points in the XYZ segmentation are NOT used! - * \return true if success. - * \note Converts from m to cm. - */ - bool - segment (const CloudXYZRGBAConstPtr& cloud_in, - CloudXYZRGBNormalPtr& cloud_out, - CloudXYZRGBNormalPtr& cloud_discarded) const; - - /** \brief Calculate the normals of the input cloud. - * \param[in] cloud_in The input cloud. - * \param[out] cloud_out Input cloud with normals appended. - * \return true if success. - * \note Converts from m to cm. - */ - bool - calculateNormals (const CloudXYZRGBAConstPtr& cloud_in, - CloudXYZRGBNormalPtr& cloud_out) const; - - /** @{ */ - /** \brief Points outside of X - Y - Z - min / max are discarded. The unit is cm. The min values must be smaller than the max values. */ - inline void setXMin (const float x_min) {if (x_min < x_max_) x_min_ = x_min;} - inline void setXMax (const float x_max) {if (x_max > x_min_) x_max_ = x_max;} - inline void setYMin (const float y_min) {if (y_min < y_max_) y_min_ = y_min;} - inline void setYMax (const float y_max) {if (y_max > y_min_) y_max_ = y_max;} - inline void setZMin (const float z_min) {if (z_min < z_max_) z_min_ = z_min;} - inline void setZMax (const float z_max) {if (z_max > z_min_) z_max_ = z_max;} - - inline float getXMin () const {return (x_min_);} - inline float getXMax () const {return (x_max_);} - inline float getYMin () const {return (y_min_);} - inline float getYMax () const {return (y_max_);} - inline float getZMin () const {return (z_min_);} - inline float getZMax () const {return (z_max_);} - /** @} */ - - /** @{ */ - /** \brief Simple color segmentation in the HSV color space. Points inside of H - S - V min / max are discarded. H must be in the range 0 and 360, S and V in the range 0 and 1. - * \note If you set values outside of the allowed range the member variables are clamped to the next best value. E.g. H is set to 0 if you pass -1. - */ - inline void setHMin (const float h_min) {h_min_ = pcl::ihs::clamp (h_min, 0.f, 360.f);} - inline void setHMax (const float h_max) {h_max_ = pcl::ihs::clamp (h_max, 0.f, 360.f);} - inline void setSMin (const float s_min) {s_min_ = pcl::ihs::clamp (s_min, 0.f, 1.f);} - inline void setSMax (const float s_max) {s_max_ = pcl::ihs::clamp (s_max, 0.f, 1.f);} - inline void setVMin (const float v_min) {v_min_ = pcl::ihs::clamp (v_min, 0.f, 1.f);} - inline void setVMax (const float v_max) {v_max_ = pcl::ihs::clamp (v_max, 0.f, 1.f);} - - inline float getHMin () const {return (h_min_);} - inline float getHMax () const {return (h_max_);} - inline float getSMin () const {return (s_min_);} - inline float getSMax () const {return (s_max_);} - inline float getVMin () const {return (v_min_);} - inline float getVMax () const {return (v_max_);} - /** @} */ - - /** @{ */ - /** \brief If true the color values inside of H - S - V min / max are accepted instead of discarded. */ - inline void setColorSegmentationInverted (const bool hsv_inverted) {hsv_inverted_ = hsv_inverted;} - inline bool getColorSegmentationInverted () const {return (hsv_inverted_);} - /** @} */ - - /** @{ */ - /** \brief Enable / disable the color segmentation. */ - inline void setColorSegmentationEnabled (const bool hsv_enabled) {hsv_enabled_ = hsv_enabled;} - inline bool getColorSegmentationEnabled () const {return (hsv_enabled_);} - /** @} */ - - /** @{ */ - /** \brief The XYZ mask is eroded with a kernel of this size. */ - inline void setXYZErodeSize (const unsigned int size) {size_erode_ = size;} - inline unsigned int getXYZErodeSize () const {return (size_erode_);} - /** @} */ - - /** @{ */ - /** \brief The HSV mask is dilated with a kernel of this size. */ - inline void setHSVDilateSize (const unsigned int size) {size_dilate_ = size;} - inline unsigned int getHSVDilateSize () const {return (size_dilate_);} - /** @} */ - - private: - - using Normal = pcl::Normal; - using CloudNormals = pcl::PointCloud; - using CloudNormalsPtr = CloudNormals::Ptr; - using CloudNormalsConstPtr = CloudNormals::ConstPtr; - - using NormalEstimation = pcl::IntegralImageNormalEstimation ; - using NormalEstimationPtr = NormalEstimation::Ptr; - using NormalEstimationConstPtr = NormalEstimation::ConstPtr; - - using MatrixXb = Eigen::Matrix ; - using MatrixXi = Eigen::MatrixXi; - - /** \brief Erodes the input mask k times with a diamond shaped structuring element. - * \see http://ostermiller.org/dilate_and_erode.html - */ - void - erode (MatrixXb& mask, const int k) const; - - /** \brief Dilates the input mask k times with a diamond shaped structuring element. - * \see http://ostermiller.org/dilate_and_erode.html - */ - void - dilate (MatrixXb& mask, const int k) const; - - /** \brief Calculates the manhattan distance map for the input matrix. - * \param[in] mat Input matrix. - * \param[in] comp Compared value. mat==comp will have zero distance. - * \return Matrix containing the distances to mat==comp - * \see http://ostermiller.org/dilate_and_erode.html - */ - MatrixXi - manhattan (const MatrixXb& mat, const bool comp) const; - - /** \brief Conversion from the RGB to HSV color space. */ - void - RGBToHSV (const unsigned char r, - const unsigned char g, - const unsigned char b, - float& h, - float& s, - float& v) const; - - //////////////////////////////////////////////////////////////////////// - // Members - //////////////////////////////////////////////////////////////////////// - - NormalEstimationPtr normal_estimation_; - - float x_min_; - float x_max_; - float y_min_; - float y_max_; - float z_min_; - float z_max_; - - float h_min_; - float h_max_; - float s_min_; - float s_max_; - float v_min_; - float v_max_; - - bool hsv_inverted_; - bool hsv_enabled_; - - unsigned int size_dilate_; - unsigned int size_erode_; - }; - } // End namespace ihs +namespace pcl { +namespace ihs { +/** \brief Bundles methods that are applied to the input data from the sensor. + * \author Martin Saelzle + * \ingroup apps + */ +class PCL_EXPORTS InputDataProcessing { +public: + using PointXYZRGBA = pcl::PointXYZRGBA; + using CloudXYZRGBA = pcl::PointCloud; + using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr; + using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr; + + using PointXYZRGBNormal = pcl::PointXYZRGBNormal; + using CloudXYZRGBNormal = pcl::PointCloud; + using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; + using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; + + /** \brief Constructor */ + InputDataProcessing(); + + /** \brief Apply the segmentation on the input cloud (XYZ and HSV). + * + * \param[in] cloud_in The input cloud. + * \param[out] cloud_out The segmented cloud. + * \param[out] cloud_discarded Cloud containing all points that were removed during + * the HSV segmentation. The points in the XYZ segmentation are NOT used! + * + * \return true if success. + * + * \note Converts from m to cm. + */ + bool + segment(const CloudXYZRGBAConstPtr& cloud_in, + CloudXYZRGBNormalPtr& cloud_out, + CloudXYZRGBNormalPtr& cloud_discarded) const; + + /** \brief Calculate the normals of the input cloud. + * + * \param[in] cloud_in The input cloud. + * \param[out] cloud_out Input cloud with normals appended. + * + * \return true if success. + * + * \note Converts from m to cm. + */ + bool + calculateNormals(const CloudXYZRGBAConstPtr& cloud_in, + CloudXYZRGBNormalPtr& cloud_out) const; + + /** @{ */ + /** \brief Points outside of X - Y - Z - min / max are discarded. The unit is cm. The + * min values must be smaller than the max values. */ + inline void + setXMin(const float x_min) + { + if (x_min < x_max_) + x_min_ = x_min; + } + inline void + setXMax(const float x_max) + { + if (x_max > x_min_) + x_max_ = x_max; + } + inline void + setYMin(const float y_min) + { + if (y_min < y_max_) + y_min_ = y_min; + } + inline void + setYMax(const float y_max) + { + if (y_max > y_min_) + y_max_ = y_max; + } + inline void + setZMin(const float z_min) + { + if (z_min < z_max_) + z_min_ = z_min; + } + inline void + setZMax(const float z_max) + { + if (z_max > z_min_) + z_max_ = z_max; + } + + inline float + getXMin() const + { + return (x_min_); + } + inline float + getXMax() const + { + return (x_max_); + } + inline float + getYMin() const + { + return (y_min_); + } + inline float + getYMax() const + { + return (y_max_); + } + inline float + getZMin() const + { + return (z_min_); + } + inline float + getZMax() const + { + return (z_max_); + } + /** @} */ + + /** @{ */ + /** \brief Simple color segmentation in the HSV color space. Points inside of H - S - + * V min / max are discarded. H must be in the range 0 and 360, S and V in the range 0 + * and 1. + * + * \note If you set values outside of the allowed range the member variables + * are clamped to the next best value. E.g. H is set to 0 if you pass -1. + */ + inline void + setHMin(const float h_min) + { + h_min_ = pcl::ihs::clamp(h_min, 0.f, 360.f); + } + inline void + setHMax(const float h_max) + { + h_max_ = pcl::ihs::clamp(h_max, 0.f, 360.f); + } + inline void + setSMin(const float s_min) + { + s_min_ = pcl::ihs::clamp(s_min, 0.f, 1.f); + } + inline void + setSMax(const float s_max) + { + s_max_ = pcl::ihs::clamp(s_max, 0.f, 1.f); + } + inline void + setVMin(const float v_min) + { + v_min_ = pcl::ihs::clamp(v_min, 0.f, 1.f); + } + inline void + setVMax(const float v_max) + { + v_max_ = pcl::ihs::clamp(v_max, 0.f, 1.f); + } + + inline float + getHMin() const + { + return (h_min_); + } + inline float + getHMax() const + { + return (h_max_); + } + inline float + getSMin() const + { + return (s_min_); + } + inline float + getSMax() const + { + return (s_max_); + } + inline float + getVMin() const + { + return (v_min_); + } + inline float + getVMax() const + { + return (v_max_); + } + /** @} */ + + /** @{ */ + /** \brief If true the color values inside of H - S - V min / max are accepted instead + * of discarded. */ + inline void + setColorSegmentationInverted(const bool hsv_inverted) + { + hsv_inverted_ = hsv_inverted; + } + inline bool + getColorSegmentationInverted() const + { + return (hsv_inverted_); + } + /** @} */ + + /** @{ */ + /** \brief Enable / disable the color segmentation. */ + inline void + setColorSegmentationEnabled(const bool hsv_enabled) + { + hsv_enabled_ = hsv_enabled; + } + inline bool + getColorSegmentationEnabled() const + { + return (hsv_enabled_); + } + /** @} */ + + /** @{ */ + /** \brief The XYZ mask is eroded with a kernel of this size. */ + inline void + setXYZErodeSize(const unsigned int size) + { + size_erode_ = size; + } + inline unsigned int + getXYZErodeSize() const + { + return (size_erode_); + } + /** @} */ + + /** @{ */ + /** \brief The HSV mask is dilated with a kernel of this size. */ + inline void + setHSVDilateSize(const unsigned int size) + { + size_dilate_ = size; + } + inline unsigned int + getHSVDilateSize() const + { + return (size_dilate_); + } + /** @} */ + +private: + using Normal = pcl::Normal; + using CloudNormals = pcl::PointCloud; + using CloudNormalsPtr = CloudNormals::Ptr; + using CloudNormalsConstPtr = CloudNormals::ConstPtr; + + using NormalEstimation = pcl::IntegralImageNormalEstimation; + using NormalEstimationPtr = NormalEstimation::Ptr; + using NormalEstimationConstPtr = NormalEstimation::ConstPtr; + + using MatrixXb = Eigen::Matrix; + using MatrixXi = Eigen::MatrixXi; + + /** \brief Erodes the input mask k times with a diamond shaped structuring element. + * \see http://ostermiller.org/dilate_and_erode.html + */ + void + erode(MatrixXb& mask, const int k) const; + + /** \brief Dilates the input mask k times with a diamond shaped structuring element. + * \see http://ostermiller.org/dilate_and_erode.html + */ + void + dilate(MatrixXb& mask, const int k) const; + + /** \brief Calculates the manhattan distance map for the input matrix. + * + * \param[in] mat Input matrix. + * \param[in] comp Compared value. mat==comp will have zero distance. + * + * \return Matrix containing the distances to mat==comp + * \see http://ostermiller.org/dilate_and_erode.html + */ + MatrixXi + manhattan(const MatrixXb& mat, const bool comp) const; + + /** \brief Conversion from the RGB to HSV color space. */ + void + RGBToHSV(const unsigned char r, + const unsigned char g, + const unsigned char b, + float& h, + float& s, + float& v) const; + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + NormalEstimationPtr normal_estimation_; + + float x_min_; + float x_max_; + float y_min_; + float y_max_; + float z_min_; + float z_max_; + + float h_min_; + float h_max_; + float s_min_; + float s_max_; + float v_min_; + float v_max_; + + bool hsv_inverted_; + bool hsv_enabled_; + + unsigned int size_dilate_; + unsigned int size_erode_; +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/integration.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/integration.h index 2617e12c..4c7b37e6 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/integration.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/integration.h @@ -40,183 +40,214 @@ #pragma once -#include - #include -#include #include +#include + +#include //////////////////////////////////////////////////////////////////////////////// // Integration //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - /** \brief Integrate several clouds into a common mesh. - * \author Martin Saelzle - * \ingroup apps - */ - class PCL_EXPORTS Integration - { - public: - - using PointXYZRGBNormal = pcl::PointXYZRGBNormal; - using CloudXYZRGBNormal = pcl::PointCloud; - using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; - using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - - using Mesh = pcl::ihs::Mesh; - using MeshPtr = pcl::ihs::MeshPtr; - using MeshConstPtr = pcl::ihs::MeshConstPtr; - using VertexIndex = Mesh::VertexIndex; - using VertexIndices = Mesh::VertexIndices; - - /** \brief Constructor. */ - Integration (); - - /** \brief Reconstructs a mesh from an organized cloud. - * \param[in] cloud_data Input cloud. Must be organized. - * \param[in] mesh_model Reconstructed mesh. - * \return true if success. - */ - bool - reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_data, - MeshPtr& mesh_model) const; - - /** \brief Merge the organized cloud into the mesh. - * \param[in] cloud_data Input cloud. Must be organized. - * \param[in,out] mesh_model Mesh with new points integrated. - * \param[in] T Transformation that aligns the data cloud with the model mesh. - * \return true if success. - */ - bool - merge (const CloudXYZRGBNormalConstPtr& cloud_data, - MeshPtr& mesh_model, - const Eigen::Matrix4f& T) const; - - /** \brief Outlier rejection. In each merge step points that have not been observed again age by one iteration. Points that are observed again get an age of 0. Once a point reaches the maximum age it is decided if the point is removed or kept in the mesh. A point is removed if it has not been observed from a minimum number of directions. - * \param[in,out] mesh The mesh which should be processed. - * \param[in] cleanup Calls mesh.cleanup () if true. - */ - void - age (const MeshPtr& mesh, const bool cleanup=true) const; - - /** \brief Removes unfit vertices regardless of their age. Unfit vertices are those that have not been observed from enough directions. - * \param[in,out] mesh The which should be processed. - * \param[in] cleanup Calls mesh.cleanup () if true. - */ - void - removeUnfitVertices (const MeshPtr& mesh, const bool cleanup=true) const; - - /** @{ */ - /** \brief Corresponding points are averaged out if their distance is below a distance threshold. Else the points are added to the mesh as new vertices (Set in cm^2). - * \note Must be greater than zero. - */ - void setMaxSquaredDistance (const float squared_distance); - float getMaxSquaredDistance () const; - /** @} */ - - /** @{ */ - /** \brief Corresponding points are only averaged out if the angle between the normals is smaller than an angle threshold. - * \note Must be between 0 and 180. Values outside this range are clamped to the nearest valid value. - */ - void setMaxAngle (const float angle); - float getMaxAngle () const; - /** @} */ - - /** @{ */ - /** \brief Once a point reaches the maximum age it is decided if the point is removed or kept in the mesh. - * \note Must be greater than zero. - */ - void setMaxAge (const unsigned int age); - unsigned int getMaxAge () const; - /** @} */ - - /** @{ */ - /** \brief A point is removed if it has not been observed from a minimum number of directions. - * \note Must be greater than zero. - */ - void setMinDirections (const unsigned int directions); - unsigned int getMinDirections () const; - /** @} */ - - private: - - using PointXYZ = pcl::PointXYZ; - using CloudXYZ = pcl::PointCloud; - using CloudXYZPtr = CloudXYZ::Ptr; - using CloudXYZConstPtr = CloudXYZ::ConstPtr; - - using PointIHS = pcl::ihs::PointIHS; - using CloudIHS = pcl::ihs::CloudIHS; - using CloudIHSPtr = pcl::ihs::CloudIHSPtr; - using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; - - using KdTree = pcl::KdTree; - using KdTreePtr = KdTree::Ptr; - using KdTreeConstPtr = KdTree::ConstPtr; - - std::uint8_t - trimRGB (const float val) const; - - /** \brief Adds two triangles between points 0-1-3 and 1-2-3 to the mesh. */ - void - addToMesh (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2, - const PointIHS& pt_3, - VertexIndex& vi_0, - VertexIndex& vi_1, - VertexIndex& vi_2, - VertexIndex& vi_3, - const MeshPtr& mesh) const; - - /** \brief Adds a triangle between the points 0-1-2 to the mesh. */ - void - addToMesh (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2, - VertexIndex& vi_0, - VertexIndex& vi_1, - VertexIndex& vi_2, - const MeshPtr& mesh) const; - - /** \brief Returns true if the distance between the three points is below a threshold. */ - bool - distanceThreshold (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2) const; - - /** \brief Returns true if the distance between the four points is below a threshold. */ - bool - distanceThreshold (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2, - const PointIHS& pt_3) const; - - //////////////////////////////////////////////////////////////////////// - // Members - //////////////////////////////////////////////////////////////////////// - - /** \brief Nearest neighbor search. */ - KdTreePtr kd_tree_; - - /** \brief Maximum squared distance below which points are averaged out. */ - float max_squared_distance_; - - /** \brief Maximum angle between normals below which points are averaged out. In degrees. */ - float max_angle_; - - /** \brief Minimum weight above which points are added. */ - float min_weight_; - - /** \brief Once a point reaches the maximum age it is decided if the point is removed or kept in the mesh. */ - unsigned int max_age_; - - /** \brief A point is removed if it has not been observed from a minimum number of directions. */ - unsigned int min_directions_; - }; - } // End namespace ihs +namespace pcl { +namespace ihs { +/** \brief Integrate several clouds into a common mesh. + * \author Martin Saelzle + * \ingroup apps + */ +class PCL_EXPORTS Integration { +public: + using PointXYZRGBNormal = pcl::PointXYZRGBNormal; + using CloudXYZRGBNormal = pcl::PointCloud; + using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; + using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; + + using Mesh = pcl::ihs::Mesh; + using MeshPtr = pcl::ihs::MeshPtr; + using MeshConstPtr = pcl::ihs::MeshConstPtr; + using VertexIndex = Mesh::VertexIndex; + using VertexIndices = Mesh::VertexIndices; + + /** \brief Constructor. */ + Integration(); + + /** \brief Reconstructs a mesh from an organized cloud. + * + * \param[in] cloud_data Input cloud. Must be organized. + * \param[in] mesh_model Reconstructed mesh. + * + * \return true if success. + */ + bool + reconstructMesh(const CloudXYZRGBNormalConstPtr& cloud_data, + MeshPtr& mesh_model) const; + + /** \brief Merge the organized cloud into the mesh.# + * + * \param[in] cloud_data Input cloud. Must be organized. + * \param[in,out] mesh_model Mesh with new points integrated. + * \param[in] T Transformation that aligns the data cloud with the model mesh. + * + * \return true if success. + */ + bool + merge(const CloudXYZRGBNormalConstPtr& cloud_data, + MeshPtr& mesh_model, + const Eigen::Matrix4f& T) const; + + /** \brief Outlier rejection. In each merge step points that have not been observed + * again age by one iteration. Points that are observed again get an age of 0. Once a + * point reaches the maximum age it is decided if the point is removed or kept in the + * mesh. A point is removed if it has not been observed from a minimum number of + * directions. + * + * \param[in,out] mesh The mesh which should be processed. + * \param[in] cleanup Calls mesh.cleanup() if true. + */ + void + age(const MeshPtr& mesh, const bool cleanup = true) const; + + /** \brief Removes unfit vertices regardless of their age. Unfit vertices are those + * that have not been observed from enough directions. + * + * \param[in,out] mesh The which should be processed. + * \param[in] cleanup Calls mesh.cleanup() if true. + */ + void + removeUnfitVertices(const MeshPtr& mesh, const bool cleanup = true) const; + + /** @{ */ + /** \brief Corresponding points are averaged out if their distance is below a distance + * threshold. Else the points are added to the mesh as new vertices (Set in cm^2). + * + * \note Must be greater than zero. + */ + void + setMaxSquaredDistance(const float squared_distance); + float + getMaxSquaredDistance() const; + /** @} */ + + /** @{ */ + /** \brief Corresponding points are only averaged out if the angle between the normals + * is smaller than an angle threshold. + * + * \note Must be between 0 and 180. Values outside this range are clamped to the + * nearest valid value. + */ + void + setMaxAngle(const float angle); + float + getMaxAngle() const; + /** @} */ + + /** @{ */ + /** \brief Once a point reaches the maximum age it is decided if the point is removed + * or kept in the mesh. + * + * \note Must be greater than zero. + */ + void + setMaxAge(const unsigned int age); + unsigned int + getMaxAge() const; + /** @} */ + + /** @{ */ + /** \brief A point is removed if it has not been observed from a minimum number of + * directions. + * + * \note Must be greater than zero. + */ + void + setMinDirections(const unsigned int directions); + unsigned int + getMinDirections() const; + /** @} */ + +private: + using PointXYZ = pcl::PointXYZ; + using CloudXYZ = pcl::PointCloud; + using CloudXYZPtr = CloudXYZ::Ptr; + using CloudXYZConstPtr = CloudXYZ::ConstPtr; + + using PointIHS = pcl::ihs::PointIHS; + using CloudIHS = pcl::ihs::CloudIHS; + using CloudIHSPtr = pcl::ihs::CloudIHSPtr; + using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; + + using KdTree = pcl::KdTree; + using KdTreePtr = KdTree::Ptr; + using KdTreeConstPtr = KdTree::ConstPtr; + + std::uint8_t + trimRGB(const float val) const; + + /** \brief Adds two triangles between points 0-1-3 and 1-2-3 to the mesh. */ + void + addToMesh(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2, + const PointIHS& pt_3, + VertexIndex& vi_0, + VertexIndex& vi_1, + VertexIndex& vi_2, + VertexIndex& vi_3, + const MeshPtr& mesh) const; + + /** \brief Adds a triangle between the points 0-1-2 to the mesh. */ + void + addToMesh(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2, + VertexIndex& vi_0, + VertexIndex& vi_1, + VertexIndex& vi_2, + const MeshPtr& mesh) const; + + /** \brief Returns true if the distance between the three points is below a threshold. + */ + bool + distanceThreshold(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2) const; + + /** \brief Returns true if the distance between the four points is below a threshold. + */ + bool + distanceThreshold(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2, + const PointIHS& pt_3) const; + + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// + + /** \brief Nearest neighbor search. */ + KdTreePtr kd_tree_; + + /** \brief Maximum squared distance below which points are averaged out. */ + float max_squared_distance_; + + /** \brief Maximum angle between normals below which points are averaged out. In + * degrees. + */ + float max_angle_; + + /** \brief Minimum weight above which points are added. */ + float min_weight_; + + /** \brief Once a point reaches the maximum age it is decided if the point is removed + * or kept in the mesh. + */ + unsigned int max_age_; + + /** \brief A point is removed if it has not been observed from a minimum number of + * directions. + */ + unsigned int min_directions_; +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/main_window.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/main_window.h index 62e496e2..f8547299 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/main_window.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/main_window.h @@ -40,98 +40,120 @@ #pragma once -#include - #include +#include + //////////////////////////////////////////////////////////////////////////////// // Forward declarations //////////////////////////////////////////////////////////////////////////////// -namespace Ui -{ - class MainWindow; +namespace Ui { +class MainWindow; } -namespace pcl -{ - namespace ihs - { - class HelpWindow; - } // End namespace ihs +namespace pcl { +namespace ihs { +class HelpWindow; +} // End namespace ihs } // End namespace pcl //////////////////////////////////////////////////////////////////////////////// // MainWindow //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - class MainWindow : public QMainWindow - { - Q_OBJECT - - public: - - using InHandScanner = pcl::ihs::InHandScanner; - using HelpWindow = pcl::ihs::HelpWindow; - using RunningMode = InHandScanner::RunningMode; - - explicit MainWindow (QWidget* parent = nullptr); - ~MainWindow (); - - public Q_SLOTS: - - void showHelp (); - void saveAs (); - - // In hand scanner - void runningModeChanged (const RunningMode mode); - void keyPressEvent (QKeyEvent* event) override; - - // Input data processing. - void setXMin (const int x_min); - void setXMax (const int x_max); - void setYMin (const int y_min); - void setYMax (const int y_max); - void setZMin (const int z_min); - void setZMax (const int z_max); - - void setHMin (const int h_min); - void setHMax (const int h_max); - void setSMin (const int s_min); - void setSMax (const int s_max); - void setVMin (const int v_min); - void setVMax (const int v_max); - - void setColorSegmentationInverted (const bool is_inverted); - void setColorSegmentationEnabled (const bool is_enabled); - - void setXYZErodeSize (const int size); - void setHSVDilateSize (const int size); - - // Registration - void setEpsilon (); - void setMaxIterations (const int iterations); - void setMinOverlap (const int overlap); - void setMaxFitness (); - - void setCorrespondenceRejectionFactor (const double factor); - void setCorrespondenceRejectionMaxAngle (const int angle); - - // Integration - void setMaxSquaredDistance (); - void setAveragingMaxAngle (const int angle); - void setMaxAge (const int age); - void setMinDirections (const int directions); - - private: - - Ui::MainWindow* ui_; - HelpWindow* help_window_; - InHandScanner* ihs_; - }; - } // End namespace ihs +namespace pcl { +namespace ihs { +class MainWindow : public QMainWindow { + Q_OBJECT + +public: + using InHandScanner = pcl::ihs::InHandScanner; + using HelpWindow = pcl::ihs::HelpWindow; + using RunningMode = InHandScanner::RunningMode; + + explicit MainWindow(QWidget* parent = nullptr); + ~MainWindow() override; + +public Q_SLOTS: + + void + showHelp(); + void + saveAs(); + + // In hand scanner + void + runningModeChanged(const RunningMode mode); + void + keyPressEvent(QKeyEvent* event) override; + + // Input data processing. + void + setXMin(const int x_min); + void + setXMax(const int x_max); + void + setYMin(const int y_min); + void + setYMax(const int y_max); + void + setZMin(const int z_min); + void + setZMax(const int z_max); + + void + setHMin(const int h_min); + void + setHMax(const int h_max); + void + setSMin(const int s_min); + void + setSMax(const int s_max); + void + setVMin(const int v_min); + void + setVMax(const int v_max); + + void + setColorSegmentationInverted(const bool is_inverted); + void + setColorSegmentationEnabled(const bool is_enabled); + + void + setXYZErodeSize(const int size); + void + setHSVDilateSize(const int size); + + // Registration + void + setEpsilon(); + void + setMaxIterations(const int iterations); + void + setMinOverlap(const int overlap); + void + setMaxFitness(); + + void + setCorrespondenceRejectionFactor(const double factor); + void + setCorrespondenceRejectionMaxAngle(const int angle); + + // Integration + void + setMaxSquaredDistance(); + void + setAveragingMaxAngle(const int angle); + void + setMaxAge(const int age); + void + setMinDirections(const int directions); + +private: + Ui::MainWindow* ui_; + HelpWindow* help_window_; + InHandScanner* ihs_; +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/mesh_processing.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/mesh_processing.h index 69d785b3..fecea6f2 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/mesh_processing.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/mesh_processing.h @@ -42,33 +42,36 @@ #include -namespace pcl -{ - namespace ihs - { - /** \brief Contains methods that take advantage of the connectivity information in the mesh. - * \author Martin Saelzle - * \ingroup apps - */ - class MeshProcessing - { - public: - - using Mesh = pcl::ihs::Mesh; - using HalfEdgeIndices = Mesh::HalfEdgeIndices; +namespace pcl { +namespace ihs { +/** \brief Contains methods that take advantage of the connectivity information in the + * mesh. + * + * \author Martin Saelzle + * \ingroup apps + */ +class MeshProcessing { +public: + using Mesh = pcl::ihs::Mesh; + using HalfEdgeIndices = Mesh::HalfEdgeIndices; - static_assert (Mesh::IsManifold::value, "MeshProcessing currently works only on the manifold mesh."); + static_assert(Mesh::IsManifold::value, + "MeshProcessing currently works only on the manifold mesh."); - /** \brief Constructor. */ - MeshProcessing (); + /** \brief Constructor. */ + MeshProcessing(); - /** \brief Inserts triangles into jagged boundaries, removes isolated triangles and closes triangular holes. - * \param[in,out] mesh The mesh which should be processed. - * \param[in] boundary_collection Collection of boundary half-edges. - * \param[in] cleanup Calls mesh.cleanup () if true. - */ - void - processBoundary (Mesh& mesh, const std::vector & boundary_collection, const bool cleanup=true) const; - }; - } // End namespace ihs + /** \brief Inserts triangles into jagged boundaries, removes isolated triangles and + * closes triangular holes. + * + * \param[in,out] mesh The mesh which should be processed. + * \param[in] boundary_collection Collection of boundary half-edges. + * \param[in] cleanup Calls mesh.cleanup() if true. + */ + void + processBoundary(Mesh& mesh, + const std::vector& boundary_collection, + const bool cleanup = true) const; +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h index 895fe8a3..ed5b3490 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/offline_integration.h @@ -40,183 +40,183 @@ #pragma once +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include #include -#include #include +#include //////////////////////////////////////////////////////////////////////////////// // Forward declarations //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - class Integration; - } // End namespace ihs +namespace pcl { +namespace ihs { +class Integration; +} // End namespace ihs } // End namespace pcl //////////////////////////////////////////////////////////////////////////////// // OfflineIntegration //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - /** \brief Read the clouds and transformations from files and integrate them into one common model. - * \todo Add Documentation - */ - class PCL_EXPORTS OfflineIntegration : public pcl::ihs::OpenGLViewer - { - Q_OBJECT - - public: - - using Base = pcl::ihs::OpenGLViewer; - using Self = pcl::ihs::OfflineIntegration; - - /** \brief Constructor. */ - explicit OfflineIntegration (Base* parent=nullptr); - - /** \brief Destructor. */ - ~OfflineIntegration (); - - public Q_SLOTS: - - /** \brief Start the procedure from a path. */ - void - start (); - - private Q_SLOTS: - - /** \brief Loads in new data. */ - void - computationThread (); - - private: - - using PointXYZRGBA = pcl::PointXYZRGBA; - using CloudXYZRGBA = pcl::PointCloud; - using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr; - using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr; - - using PointXYZRGBNormal = pcl::PointXYZRGBNormal; - using CloudXYZRGBNormal = pcl::PointCloud; - using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; - using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - - using Mesh = pcl::ihs::Mesh; - using MeshPtr = pcl::ihs::MeshPtr; - using MeshConstPtr = pcl::ihs::MeshConstPtr; - - using Integration = pcl::ihs::Integration; - using IntegrationPtr = std::shared_ptr; - using IntegrationConstPtr = std::shared_ptr; - - using NormalEstimation = pcl::IntegralImageNormalEstimation ; - using NormalEstimationPtr = NormalEstimation::Ptr; - using NormalEstimationConstPtr = NormalEstimation::ConstPtr; - - /** \brief Helper object for the computation thread. Please have a look at the documentation of calcFPS. */ - class ComputationFPS : public Base::FPS - { - public: - ComputationFPS () {} - ~ComputationFPS () {} - }; - - - /** \brief Helper object for the visualization thread. Please have a look at the documentation of calcFPS. */ - class VisualizationFPS : public Base::FPS - { - public: - VisualizationFPS () {} - ~VisualizationFPS () {} - }; - - /** \brief Get a list of files with from a given directory. - * \param[in] path_dir Path to search for the files. - * \param[in] extension File extension (must start with a dot). E.g. '.pcd'. - * \param[out] files Paths to the files. - * \return True if success. - */ - bool - getFilesFromDirectory (const std::string& path_dir, - const std::string& extension, - std::vector & files) const; - - /** \brief Load the transformation matrix from the given file. - * \param[in] filename Path to the file. - * \param[out] transform The loaded transform. - * \return True if success. - */ - bool - loadTransform (const std::string& filename, - Eigen::Matrix4f& transform) const; - - /** \brief Load the cloud and transformation from the files and compute the normals. - * \param[in] filename Path to the pcd file. - * \param[out] cloud Cloud with computed normals. - * \param[out] T Loaded transformation. - * \return True if success. - */ - bool - load (const std::string& filename, - CloudXYZRGBNormalPtr& cloud, - Eigen::Matrix4f& T) const; - - /** \see http://doc.qt.digia.com/qt/qwidget.html#paintEvent - * \see http://doc.qt.digia.com/qt/opengl-overpainting.html - */ - void - paintEvent (QPaintEvent* event) override; - - /** \see http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent */ - void - keyPressEvent (QKeyEvent* event) override; - - ////////////////////////////////////////////////////////////////////////// - // Members - ////////////////////////////////////////////////////////////////////////// - - /** \brief Synchronization. */ - std::mutex mutex_; - - /** \brief Wait until the data finished processing. */ - std::mutex mutex_quit_; - - /** \brief Please have a look at the documentation of ComputationFPS. */ - ComputationFPS computation_fps_; - - /** \brief Please have a look at the documentation of VisualizationFPS. */ - VisualizationFPS visualization_fps_; - - /** \brief Path to the pcd and transformation files. */ - std::string path_dir_; - - /** \brief Model to which new data is registered to (stored as a mesh). */ - MeshPtr mesh_model_; - - /** \brief Compute the normals for the input clouds. */ - NormalEstimationPtr normal_estimation_; - - /** \brief Integrate the data cloud into a common model. */ - IntegrationPtr integration_; - - /** \brief Prevent the application to crash while closing. */ - bool destructor_called_; - - public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - } // End namespace ihs +namespace pcl { +namespace ihs { +/** \brief Read the clouds and transformations from files and integrate them into one + * common model. + * + * \todo Add Documentation + */ +class PCL_EXPORTS OfflineIntegration : public pcl::ihs::OpenGLViewer { + Q_OBJECT + +public: + using Base = pcl::ihs::OpenGLViewer; + using Self = pcl::ihs::OfflineIntegration; + + /** \brief Constructor. */ + explicit OfflineIntegration(Base* parent = nullptr); + + /** \brief Destructor. */ + ~OfflineIntegration() override; + +public Q_SLOTS: + + /** \brief Start the procedure from a path. */ + void + start(); + +private Q_SLOTS: + + /** \brief Loads in new data. */ + void + computationThread(); + +private: + using PointXYZRGBA = pcl::PointXYZRGBA; + using CloudXYZRGBA = pcl::PointCloud; + using CloudXYZRGBAPtr = CloudXYZRGBA::Ptr; + using CloudXYZRGBAConstPtr = CloudXYZRGBA::ConstPtr; + + using PointXYZRGBNormal = pcl::PointXYZRGBNormal; + using CloudXYZRGBNormal = pcl::PointCloud; + using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; + using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; + + using Mesh = pcl::ihs::Mesh; + using MeshPtr = pcl::ihs::MeshPtr; + using MeshConstPtr = pcl::ihs::MeshConstPtr; + + using Integration = pcl::ihs::Integration; + using IntegrationPtr = std::shared_ptr; + using IntegrationConstPtr = std::shared_ptr; + + using NormalEstimation = + pcl::IntegralImageNormalEstimation; + using NormalEstimationPtr = NormalEstimation::Ptr; + using NormalEstimationConstPtr = NormalEstimation::ConstPtr; + + /** \brief Helper object for the computation thread. Please have a look at the + * documentation of calcFPS. */ + class ComputationFPS : public Base::FPS { + public: + ComputationFPS() = default; + ~ComputationFPS() = default; + }; + + /** \brief Helper object for the visualization thread. Please have a look at the + * documentation of calcFPS. */ + class VisualizationFPS : public Base::FPS { + public: + VisualizationFPS() = default; + ~VisualizationFPS() = default; + }; + + /** \brief Get a list of files with from a given directory. + * + * \param[in] path_dir Path to search for the files. + * \param[in] extension File extension (must start with a dot). E.g. '.pcd'. + * \param[out] files Paths to the files. + * + * \return True if success. + */ + bool + getFilesFromDirectory(const std::string& path_dir, + const std::string& extension, + std::vector& files) const; + + /** \brief Load the transformation matrix from the given file. + * + * \param[in] filename Path to the file. + * \param[out] transform The loaded transform. + * + * \return True if success. + */ + bool + loadTransform(const std::string& filename, Eigen::Matrix4f& transform) const; + + /** \brief Load the cloud and transformation from the files and compute the normals. + * + * \param[in] filename Path to the pcd file. + * \param[out] cloud Cloud with computed normals. + * \param[out] T Loaded transformation. + * + * \return True if success. + */ + bool + load(const std::string& filename, + CloudXYZRGBNormalPtr& cloud, + Eigen::Matrix4f& T) const; + + /** \see http://doc.qt.digia.com/qt/qwidget.html#paintEvent + * \see http://doc.qt.digia.com/qt/opengl-overpainting.html + */ + void + paintEvent(QPaintEvent* event) override; + + /** \see http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent */ + void + keyPressEvent(QKeyEvent* event) override; + + ////////////////////////////////////////////////////////////////////////// + // Members + ////////////////////////////////////////////////////////////////////////// + + /** \brief Synchronization. */ + std::mutex mutex_; + + /** \brief Wait until the data finished processing. */ + std::mutex mutex_quit_; + + /** \brief Please have a look at the documentation of ComputationFPS. */ + ComputationFPS computation_fps_; + + /** \brief Please have a look at the documentation of VisualizationFPS. */ + VisualizationFPS visualization_fps_; + + /** \brief Path to the pcd and transformation files. */ + std::string path_dir_; + + /** \brief Model to which new data is registered to (stored as a mesh). */ + MeshPtr mesh_model_; + + /** \brief Compute the normals for the input clouds. */ + NormalEstimationPtr normal_estimation_; + + /** \brief Integrate the data cloud into a common model. */ + IntegrationPtr integration_; + + /** \brief Prevent the application to crash while closing. */ + bool destructor_called_; + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h index 46f73b21..d9b94d72 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/opengl_viewer.h @@ -40,414 +40,445 @@ #pragma once +#include +#include #include #include #include -#include -#include #include -#include #include +#include #include #include -namespace pcl -{ - namespace ihs - { - namespace detail +namespace pcl { +namespace ihs { +namespace detail { +/** \brief Mesh format more efficient for visualization than the half-edge data + * structure. \see http://en.wikipedia.org/wiki/Polygon_mesh#Face-vertex_meshes + * + * \note Only triangles are currently supported. + */ +class FaceVertexMesh { +public: + class Triangle { + public: + Triangle() : first(0), second(0), third(0) {} + Triangle(const unsigned int first, + const unsigned int second, + const unsigned int third) + : first(first), second(second), third(third) + {} + + unsigned int first; + unsigned int second; + unsigned int third; + }; + + /** \brief Constructor */ + FaceVertexMesh(); + + /** \brief Constructor. Converts the input mesh into a face vertex mesh. */ + FaceVertexMesh(const Mesh& mesh, const Eigen::Isometry3d& T); + + using PointIHS = pcl::ihs::PointIHS; + using CloudIHS = pcl::ihs::CloudIHS; + using CloudIHSPtr = pcl::ihs::CloudIHSPtr; + using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; + + CloudIHS vertices; + std::vector triangles; + Eigen::Isometry3d transformation; + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // End namespace detail + +/** \brief Viewer for the in-hand scanner based on Qt and OpenGL. + * + * \note Currently you have to derive from this class to use it. Implement the + * paintEvent: Call the paint event of this class and declare a QPainter. + */ +class PCL_EXPORTS OpenGLViewer : public QGLWidget { + Q_OBJECT + +public: + using PointXYZRGBNormal = pcl::PointXYZRGBNormal; + using CloudXYZRGBNormal = pcl::PointCloud; + using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; + using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; + + using Mesh = pcl::ihs::Mesh; + using MeshPtr = pcl::ihs::MeshPtr; + using MeshConstPtr = pcl::ihs::MeshConstPtr; + + /** \brief How to draw the mesh. */ + enum MeshRepresentation { + MR_POINTS, /**< Draw the points. */ + MR_EDGES, /**< Wireframe represen of the mesh. */ + MR_FACES /**< Draw the faces of the mesh without edges. */ + }; + + /** \brief How to color the shapes. */ + enum Coloring { + COL_RGB, /**< Coloring according to the rgb values. */ + COL_ONE_COLOR, /**< Use one color for all points. */ + COL_VISCONF /**< Coloring according to the visibility confidence. */ + }; + + /** \brief Coefficients for the wireframe box. */ + class BoxCoefficients { + public: + BoxCoefficients() + : x_min(0) + , x_max(0) + , y_min(0) + , y_max(0) + , z_min(0) + , z_max(0) + , transformation(Eigen::Isometry3d::Identity()) + {} + + BoxCoefficients(const float x_min, + const float x_max, + const float y_min, + const float y_max, + const float z_min, + const float z_max, + const Eigen::Isometry3d& T) + : x_min(x_min) + , x_max(x_max) + , y_min(y_min) + , y_max(y_max) + , z_min(z_min) + , z_max(z_max) + , transformation(T) + {} + + float x_min; + float x_max; + float y_min; + float y_max; + float z_min; + float z_max; + Eigen::Isometry3d transformation; + + public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + }; + + /** \brief Constructor. */ + explicit OpenGLViewer(QWidget* parent = nullptr); + + /** \brief Destructor. */ + ~OpenGLViewer() override; + + /** \brief Add a mesh to be drawn. + * + * \param[in] mesh The input mesh. + * \param[in] id Unique identifier for the mesh. The internal mesh is replaced by the + * input mesh if the id already exists. + * \param[in] T Transformation applied to the mesh. Defaults to an identity + * transformation. + * + * \return true if success. + * + * \note Converts the mesh to the internal representation better suited for + * visualization. Therefore this method takes some time. + */ + bool + addMesh(const MeshConstPtr& mesh, + const std::string& id, + const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()); + + /** \brief Convert an organized cloud to a mesh and draw it. + * + * \param[in] cloud Organized input cloud. + * \param[in] id Unique identifier for the mesh. The internal mesh is replaced by the + * converted input mesh if the id already exists. + * \param[in] T Transformation applied to the mesh. Defaults to an identity + * transformation. + * + * \return true if success. + * + * \note This method takes some time for the conversion). + */ + bool + addMesh(const CloudXYZRGBNormalConstPtr& cloud, + const std::string& id, + const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()); + + /** \brief Remove the mesh with the given id. + * + * \param[in] id Identifier of the mesh (results in a failure if the id does not + * exist). + * + * \return true if success. + */ + bool + removeMesh(const std::string& id); + + /** \brief Remove all meshes that are currently drawn. */ + void + removeAllMeshes(); + + /** \brief Set the coefficients for the box. */ + void + setBoxCoefficients(const BoxCoefficients& coeffs); + + /** \brief Enable / disable drawing the box. */ + void + setDrawBox(const bool enabled); + + /** \brief Check if the box is drawn. */ + bool + getDrawBox() const; + + /** \brief Set the point around which the camera rotates during mouse navigation. */ + void + setPivot(const Eigen::Vector3d& pivot); + + /** \brief Searches the given id in the drawn meshes and calculates the pivot as the + * centroid of the found geometry. + * + * \note Returns immediately and computes the pivot in + * another thread. + */ + void + setPivot(const std::string& id); + + /** \brief Stop the visualization timer. */ + void + stopTimer(); + + /** \brief The visibility confidence is normalized with this value (must be greater + * than 1). */ + void + setVisibilityConfidenceNormalization(const float vis_conf_norm); + + /** \see http://doc.qt.digia.com/qt/qwidget.html#minimumSizeHint-prop */ + QSize + minimumSizeHint() const override; + + /** \see http://doc.qt.digia.com/qt/qwidget.html#sizeHint-prop */ + QSize + sizeHint() const override; + + /** \brief Set the scaling factor to convert from meters to the unit of the drawn + * files. */ + void + setScalingFactor(const double scale); + +public Q_SLOTS: + + /** \brief Requests the scene to be re-drawn (called periodically from a timer). */ + void + timerCallback(); + + /** \brief Reset the virtual camera position and orientation. */ + void + resetCamera(); + + /** \brief Toggle the mesh representation. */ + void + toggleMeshRepresentation(); + + /** \brief Set the mesh representation. */ + void + setMeshRepresentation(const MeshRepresentation& representation); + + /** \brief Toggle the coloring mode. */ + void + toggleColoring(); + + /** \brief Set the coloring mode. */ + void + setColoring(const Coloring& coloring); + +protected: + /** \brief Please have a look at the documentation of calcFPS. */ + class FPS { + public: + FPS() : fps_(0.) {} + + inline double& + value() { - /** \brief Mesh format more efficient for visualization than the half-edge data structure. - * \see http://en.wikipedia.org/wiki/Polygon_mesh#Face-vertex_meshes - * \note Only triangles are currently supported. - */ - class FaceVertexMesh - { - public: - - class Triangle - { - public: - - Triangle () : first (0), second (0), third (0) {} - Triangle (const unsigned int first, const unsigned int second, const unsigned int third) - : first (first), second (second), third (third) - { - } - - unsigned int first; - unsigned int second; - unsigned int third; - }; - - /** \brief Constructor */ - FaceVertexMesh (); - - /** \brief Constructor. Converts the input mesh into a face vertex mesh. */ - FaceVertexMesh (const Mesh& mesh, const Eigen::Isometry3d& T); - - using PointIHS = pcl::ihs::PointIHS; - using CloudIHS = pcl::ihs::CloudIHS; - using CloudIHSPtr = pcl::ihs::CloudIHSPtr; - using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; - - CloudIHS vertices; - std::vector triangles; - Eigen::Isometry3d transformation; - - public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - } // End namespace detail - - /** \brief Viewer for the in-hand scanner based on Qt and OpenGL. - * \note Currently you have to derive from this class to use it. Implement the paintEvent: Call the paint event of this class and declare a QPainter. - */ - class PCL_EXPORTS OpenGLViewer : public QGLWidget + return (fps_); + } + inline double + value() const { - Q_OBJECT - - public: - - using PointXYZRGBNormal = pcl::PointXYZRGBNormal; - using CloudXYZRGBNormal = pcl::PointCloud; - using CloudXYZRGBNormalPtr = CloudXYZRGBNormal::Ptr; - using CloudXYZRGBNormalConstPtr = CloudXYZRGBNormal::ConstPtr; - - using Mesh = pcl::ihs::Mesh; - using MeshPtr = pcl::ihs::MeshPtr; - using MeshConstPtr = pcl::ihs::MeshConstPtr; - - /** \brief How to draw the mesh. */ - enum MeshRepresentation - { - MR_POINTS, /**< Draw the points. */ - MR_EDGES, /**< Wireframe represen of the mesh. */ - MR_FACES /**< Draw the faces of the mesh without edges. */ - }; - - /** \brief How to color the shapes. */ - enum Coloring - { - COL_RGB, /**< Coloring according to the rgb values. */ - COL_ONE_COLOR, /**< Use one color for all points. */ - COL_VISCONF /**< Coloring according to the visibility confidence. */ - }; - - /** \brief Coefficients for the wireframe box. */ - class BoxCoefficients - { - public: - - BoxCoefficients () - : x_min (0), x_max (0), - y_min (0), y_max (0), - z_min (0), z_max (0), - transformation (Eigen::Isometry3d::Identity ()) - { - } - - BoxCoefficients (const float x_min, const float x_max, - const float y_min, const float y_max, - const float z_min, const float z_max, - const Eigen::Isometry3d& T) - : x_min (x_min), x_max (x_max), - y_min (y_min), y_max (y_max), - z_min (z_min), z_max (z_max), - transformation (T) - { - } - - float x_min; float x_max; - float y_min; float y_max; - float z_min; float z_max; - Eigen::Isometry3d transformation; - - public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - - /** \brief Constructor. */ - explicit OpenGLViewer (QWidget* parent=nullptr); - - /** \brief Destructor. */ - ~OpenGLViewer (); - - /** \brief Add a mesh to be drawn. - * \param[in] mesh The input mesh. - * \param[in] id Unique identifier for the mesh. The internal mesh is replaced by the input mesh if the id already exists. - * \param[in] T Transformation applied to the mesh. Defaults to an identity transformation. - * \return true if success. - * \note Converts the mesh to the internal representation better suited for visualization. Therefore this method takes some time. - */ - bool - addMesh (const MeshConstPtr& mesh, const std::string& id, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity ()); - - /** \brief Convert an organized cloud to a mesh and draw it. - * \param[in] cloud Organized input cloud. - * \param[in] id Unique identifier for the mesh. The internal mesh is replaced by the converted input mesh if the id already exists. - * \param[in] T Transformation applied to the mesh. Defaults to an identity transformation. - * \return true if success. - * \note This method takes some time for the conversion). - */ - bool - addMesh (const CloudXYZRGBNormalConstPtr& cloud, const std::string& id, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity ()); - - /** \brief Remove the mesh with the given id. - * \param[in] id Identifier of the mesh (results in a failure if the id does not exist). - * \return true if success. - */ - bool - removeMesh (const std::string& id); - - /** \brief Remove all meshes that are currently drawn. */ - void - removeAllMeshes (); - - /** \brief Set the coefficients for the box. */ - void - setBoxCoefficients (const BoxCoefficients& coeffs); - - /** \brief Enable / disable drawing the box. */ - void - setDrawBox (const bool enabled); - - /** \brief Check if the box is drawn. */ - bool - getDrawBox () const; - - /** \brief Set the point around which the camera rotates during mouse navigation. */ - void - setPivot (const Eigen::Vector3d& pivot); - - /** \brief Searches the given id in the drawn meshes and calculates the pivot as the centroid of the found geometry. - * \note Returns immediately and computes the pivot in another thread. - */ - void - setPivot (const std::string& id); - - /** \brief Stop the visualization timer. */ - void - stopTimer (); - - /** \brief The visibility confidence is normalized with this value (must be greater than 1). */ - void - setVisibilityConfidenceNormalization (const float vis_conf_norm); - - /** \see http://doc.qt.digia.com/qt/qwidget.html#minimumSizeHint-prop */ - QSize - minimumSizeHint () const override; - - /** \see http://doc.qt.digia.com/qt/qwidget.html#sizeHint-prop */ - QSize - sizeHint () const override; - - /** \brief Set the scaling factor to convert from meters to the unit of the drawn files. */ - void - setScalingFactor (const double scale); - - public Q_SLOTS: - - /** \brief Requests the scene to be re-drawn (called periodically from a timer). */ - void - timerCallback (); - - /** \brief Reset the virtual camera position and orientation. */ - void - resetCamera (); - - /** \brief Toggle the mesh representation. */ - void - toggleMeshRepresentation (); - - /** \brief Set the mesh representation. */ - void - setMeshRepresentation (const MeshRepresentation& representation); - - /** \brief Toggle the coloring mode. */ - void - toggleColoring (); - - /** \brief Set the coloring mode. */ - void - setColoring (const Coloring& coloring); - - protected: - - /** \brief Please have a look at the documentation of calcFPS. */ - class FPS - { - public: - - FPS () : fps_ (0.) {} - - inline double& value () {return (fps_);} - inline double value () const {return (fps_);} - - inline std::string - str () const - { - std::stringstream ss; - ss << std::setprecision (1) << std::fixed << fps_; - return (ss.str ()); - } - - protected: - - ~FPS () {} - - private: - - double fps_; - }; + return (fps_); + } - /** Measures the performance of the current thread (selected by passing the corresponding 'fps' helper object). The resulting value is stored in the fps object. */ - template void - calcFPS (FPS& fps) const - { - static pcl::StopWatch sw; - static unsigned int count = 0; - - ++count; - if (sw.getTimeSeconds () >= .2) - { - fps.value () = static_cast (count) / sw.getTimeSeconds (); - count = 0; - sw.reset (); - } - } - - /** \see http://doc.qt.digia.com/qt/qwidget.html#paintEvent - * \see http://doc.qt.digia.com/qt/opengl-overpainting.html - */ - void - paintEvent (QPaintEvent* event) override; + inline std::string + str() const + { + std::stringstream ss; + ss << std::setprecision(1) << std::fixed << fps_; + return (ss.str()); + } + + protected: + ~FPS() = default; + + private: + double fps_; + }; + + /** Measures the performance of the current thread (selected by passing the + * corresponding 'fps' helper object). The resulting value is stored in the fps + * object. */ + template + void + calcFPS(FPS& fps) const + { + static pcl::StopWatch sw; + static unsigned int count = 0; + + ++count; + if (sw.getTimeSeconds() >= .2) { + fps.value() = static_cast(count) / sw.getTimeSeconds(); + count = 0; + sw.reset(); + } + } - private: + /** \see http://doc.qt.digia.com/qt/qwidget.html#paintEvent + * \see http://doc.qt.digia.com/qt/opengl-overpainting.html + */ + void + paintEvent(QPaintEvent* event) override; - using Color = Eigen::Matrix ; - using Colors = Eigen::Matrix ; - using Colormap = Eigen::Matrix ; +private: + using Color = Eigen::Matrix; + using Colors = Eigen::Matrix; + using Colormap = Eigen::Matrix; - using CloudXYZRGBNormalMap = std::unordered_map ; + using CloudXYZRGBNormalMap = std::unordered_map; - using PointIHS = pcl::ihs::PointIHS; - using CloudIHS = pcl::ihs::CloudIHS; - using CloudIHSPtr = pcl::ihs::CloudIHSPtr; - using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; + using PointIHS = pcl::ihs::PointIHS; + using CloudIHS = pcl::ihs::CloudIHS; + using CloudIHSPtr = pcl::ihs::CloudIHSPtr; + using CloudIHSConstPtr = pcl::ihs::CloudIHSConstPtr; - using FaceVertexMesh = pcl::ihs::detail::FaceVertexMesh; - using FaceVertexMeshPtr = std::shared_ptr; - using FaceVertexMeshConstPtr = std::shared_ptr; - using FaceVertexMeshMap = std::unordered_map ; + using FaceVertexMesh = pcl::ihs::detail::FaceVertexMesh; + using FaceVertexMeshPtr = std::shared_ptr; + using FaceVertexMeshConstPtr = std::shared_ptr; + using FaceVertexMeshMap = std::unordered_map; - /** \brief Check if the mesh with the given id is added. - * \note Must lock the mutex before calling this method. - */ - bool - getMeshIsAdded (const std::string& id); + /** \brief Check if the mesh with the given id is added. + * + * \note Must lock the mutex before calling this method. + */ + bool + getMeshIsAdded(const std::string& id); - /** \brief Calculate the pivot for the stored id. */ - void - calcPivot (); + /** \brief Calculate the pivot for the stored id. */ + void + calcPivot(); - /** \brief Draw all meshes. - * \note Only triangle meshes are currently supported. - */ - void - drawMeshes (); + /** \brief Draw all meshes. + * + * \note Only triangle meshes are currently supported. + */ + void + drawMeshes(); - /** \brief Draw a wireframe box. */ - void - drawBox (); + /** \brief Draw a wireframe box. */ + void + drawBox(); - /** \see http://doc.qt.digia.com/qt/qglwidget.html#initializeGL */ - void - initializeGL () override; + /** \see http://doc.qt.digia.com/qt/qglwidget.html#initializeGL */ + void + initializeGL() override; - /** \see http://www.opengl.org/sdk/docs/man/xhtml/glViewport.xml */ - void - setupViewport (const int w, const int h); + /** \see http://www.opengl.org/sdk/docs/man/xhtml/glViewport.xml */ + void + setupViewport(const int w, const int h); - /** \see http://doc.qt.digia.com/qt/qglwidget.html#resizeGL */ - void - resizeGL (int w, int h) override; + /** \see http://doc.qt.digia.com/qt/qglwidget.html#resizeGL */ + void + resizeGL(int w, int h) override; - /** \see http://doc.qt.digia.com/qt/qwidget.html#mousePressEvent */ - void - mousePressEvent (QMouseEvent* event) override; + /** \see http://doc.qt.digia.com/qt/qwidget.html#mousePressEvent */ + void + mousePressEvent(QMouseEvent* event) override; - /** \see http://doc.qt.digia.com/qt/qwidget.html#mouseMoveEvent */ - void - mouseMoveEvent (QMouseEvent* event) override; + /** \see http://doc.qt.digia.com/qt/qwidget.html#mouseMoveEvent */ + void + mouseMoveEvent(QMouseEvent* event) override; - /** \see http://doc.qt.digia.com/qt/qwidget.html#wheelEvent */ - void - wheelEvent (QWheelEvent* event) override; + /** \see http://doc.qt.digia.com/qt/qwidget.html#wheelEvent */ + void + wheelEvent(QWheelEvent* event) override; - //////////////////////////////////////////////////////////////////////// - // Members - //////////////////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////////////////// + // Members + //////////////////////////////////////////////////////////////////////// - /** \brief Synchronization. */ - std::mutex mutex_vis_; + /** \brief Synchronization. */ + std::mutex mutex_vis_; - /** \brief Visualization timer. */ - std::shared_ptr timer_vis_; + /** \brief Visualization timer. */ + std::shared_ptr timer_vis_; - /** \brief Colormap. */ - Colormap colormap_; + /** \brief Colormap. */ + Colormap colormap_; - /** \brief The visibility confidence is normalized with this value. */ - float vis_conf_norm_; + /** \brief The visibility confidence is normalized with this value. */ + float vis_conf_norm_; - /** \brief Meshes stored for visualization. */ - FaceVertexMeshMap drawn_meshes_; + /** \brief Meshes stored for visualization. */ + FaceVertexMeshMap drawn_meshes_; - /** \brief How to draw the mesh. */ - MeshRepresentation mesh_representation_; + /** \brief How to draw the mesh. */ + MeshRepresentation mesh_representation_; - /** \brief How to color the shapes. */ - Coloring coloring_; + /** \brief How to color the shapes. */ + Coloring coloring_; - /** \brief A box is drawn if this value is true. */ - bool draw_box_; + /** \brief A box is drawn if this value is true. */ + bool draw_box_; - /** \brief Coefficients of the drawn box. */ - BoxCoefficients box_coefficients_; + /** \brief Coefficients of the drawn box. */ + BoxCoefficients box_coefficients_; - /** \brief Scaling factor to convert from meters to the unit of the drawn files. */ - double scaling_factor_; + /** \brief Scaling factor to convert from meters to the unit of the drawn files. */ + double scaling_factor_; - /** \brief Rotation of the camera. */ - Eigen::Quaterniond R_cam_; + /** \brief Rotation of the camera. */ + Eigen::Quaterniond R_cam_; - /** \brief Translation of the camera. */ - Eigen::Vector3d t_cam_; + /** \brief Translation of the camera. */ + Eigen::Vector3d t_cam_; - /** \brief Center of rotation during mouse navigation. */ - Eigen::Vector3d cam_pivot_; + /** \brief Center of rotation during mouse navigation. */ + Eigen::Vector3d cam_pivot_; - /** \brief Compute the pivot for the mesh with the given id. */ - std::string cam_pivot_id_; + /** \brief Compute the pivot for the mesh with the given id. */ + std::string cam_pivot_id_; - /** \brief Set to true right after the mouse got pressed and false if the mouse got moved. */ - bool mouse_pressed_begin_; + /** \brief Set to true right after the mouse got pressed and false if the mouse got + * moved. */ + bool mouse_pressed_begin_; - /** \brief Mouse x-position of the previous mouse move event. */ - int x_prev_; + /** \brief Mouse x-position of the previous mouse move event. */ + int x_prev_; - /** \brief Mouse y-position of the previous mouse move event. */ - int y_prev_; + /** \brief Mouse y-position of the previous mouse move event. */ + int y_prev_; - public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; - } // End namespace ihs +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; +} // End namespace ihs } // End namespace pcl // http://doc.qt.digia.com/qt/qmetatype.html#Q_DECLARE_METATYPE -Q_DECLARE_METATYPE (pcl::ihs::OpenGLViewer::MeshRepresentation) -Q_DECLARE_METATYPE (pcl::ihs::OpenGLViewer::Coloring) +Q_DECLARE_METATYPE(pcl::ihs::OpenGLViewer::MeshRepresentation) +Q_DECLARE_METATYPE(pcl::ihs::OpenGLViewer::Coloring) diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/utils.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/utils.h index 937734a6..7687a794 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/utils.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/utils.h @@ -40,15 +40,16 @@ #pragma once -namespace pcl +namespace pcl { +namespace ihs { +/** \brief Clamp the value to the given range. All values smaller than min are set to + * min and all values bigger than max are set to max. + */ +template +inline T +clamp(const T value, const T min, const T max) { - namespace ihs - { - /** \brief Clamp the value to the given range. All values smaller than min are set to min and all values bigger than max are set to max. */ - template inline T - clamp (const T value, const T min, const T max) - { - return (value < min ? min : value > max ? max : value); - } - } // End namespace ihs + return (value < min ? min : value > max ? max : value); +} +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h index ef4ed284..16d97412 100644 --- a/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h +++ b/apps/in_hand_scanner/include/pcl/apps/in_hand_scanner/visibility_confidence.h @@ -40,48 +40,43 @@ #pragma once -#include - #include #include #include -namespace pcl -{ - namespace ihs - { - // - Frequency 3 Icosahedron where each vertex corresponds to a viewing direction - // - First vertex aligned to z-axis - // - Removed vertices with z < 0.3 - // -> 31 directions, fitting nicely into a 32 bit integer - // -> Very oblique angles are not considered - class PCL_EXPORTS Dome - { - public: - - static const int num_directions = 31; - using Vertices = Eigen::Matrix ; +#include - Dome (); +namespace pcl { +namespace ihs { +// - Frequency 3 Icosahedron where each vertex corresponds to a viewing direction +// - First vertex aligned to z-axis +// - Removed vertices with z < 0.3 +// -> 31 directions, fitting nicely into a 32 bit integer +// -> Very oblique angles are not considered +class PCL_EXPORTS Dome { +public: + static const int num_directions = 31; + using Vertices = Eigen::Matrix; - Vertices - getVertices () const; + Dome(); - private: + Vertices + getVertices() const; - Vertices vertices_; +private: + Vertices vertices_; - public: - PCL_MAKE_ALIGNED_OPERATOR_NEW - }; +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; - PCL_EXPORTS void - addDirection (const Eigen::Vector4f& normal, - const Eigen::Vector4f& direction, - std::uint32_t& directions); +PCL_EXPORTS void +addDirection(const Eigen::Vector4f& normal, + const Eigen::Vector4f& direction, + std::uint32_t& directions); - PCL_EXPORTS unsigned int - countDirections (const std::uint32_t directions); +PCL_EXPORTS unsigned int +countDirections(const std::uint32_t directions); - } // End namespace ihs +} // End namespace ihs } // End namespace pcl diff --git a/apps/in_hand_scanner/src/help_window.cpp b/apps/in_hand_scanner/src/help_window.cpp index 63a24bd8..95b73bca 100644 --- a/apps/in_hand_scanner/src/help_window.cpp +++ b/apps/in_hand_scanner/src/help_window.cpp @@ -38,24 +38,20 @@ * */ - #include + #include "ui_help_window.h" //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::HelpWindow::HelpWindow (QWidget *parent) - : QDialog (parent), - ui (new Ui::HelpWindow) +pcl::ihs::HelpWindow::HelpWindow(QWidget* parent) +: QDialog(parent), ui(new Ui::HelpWindow) { ui->setupUi(this); } //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::HelpWindow::~HelpWindow() -{ - delete ui; -} +pcl::ihs::HelpWindow::~HelpWindow() { delete ui; } //////////////////////////////////////////////////////////////////////////////// diff --git a/apps/in_hand_scanner/src/icp.cpp b/apps/in_hand_scanner/src/icp.cpp index 8f21a802..b3ae1372 100644 --- a/apps/in_hand_scanner/src/icp.cpp +++ b/apps/in_hand_scanner/src/icp.cpp @@ -39,43 +39,43 @@ */ #include - -#include -#include -#include -#include - -#include +#include #include #include +#include -#include +#include +#include +#include +#include //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::ICP::ICP () - : kd_tree_ (new pcl::KdTreeFLANN ()), +pcl::ihs::ICP::ICP() +: kd_tree_(new pcl::KdTreeFLANN()) +, - epsilon_ (10e-6f), - max_iterations_ (50), - min_overlap_ (.75f), - max_fitness_ (.1f), +epsilon_(10e-6f) +, max_iterations_(50) +, min_overlap_(.75f) +, max_fitness_(.1f) +, - factor_ (9.f), - max_angle_ (45.f) -{ -} +factor_(9.f) +, max_angle_(45.f) +{} //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::ICP::setEpsilon (const float epsilon) +pcl::ihs::ICP::setEpsilon(const float epsilon) { - if (epsilon > 0) epsilon_ = epsilon; + if (epsilon > 0) + epsilon_ = epsilon; } float -pcl::ihs::ICP::getEpsilon () const +pcl::ihs::ICP::getEpsilon() const { return (epsilon_); } @@ -83,13 +83,13 @@ pcl::ihs::ICP::getEpsilon () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::ICP::setMaxIterations (const unsigned int max_iter) +pcl::ihs::ICP::setMaxIterations(const unsigned int max_iter) { max_iterations_ = max_iter < 1 ? 1 : max_iter; } unsigned int -pcl::ihs::ICP::getMaxIterations () const +pcl::ihs::ICP::getMaxIterations() const { return (max_iterations_); } @@ -97,13 +97,13 @@ pcl::ihs::ICP::getMaxIterations () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::ICP::setMinOverlap (const float overlap) +pcl::ihs::ICP::setMinOverlap(const float overlap) { - min_overlap_ = pcl::ihs::clamp (overlap, 0.f, 1.f); + min_overlap_ = pcl::ihs::clamp(overlap, 0.f, 1.f); } float -pcl::ihs::ICP::getMinOverlap () const +pcl::ihs::ICP::getMinOverlap() const { return (min_overlap_); } @@ -111,13 +111,14 @@ pcl::ihs::ICP::getMinOverlap () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::ICP::setMaxFitness (const float fitness) +pcl::ihs::ICP::setMaxFitness(const float fitness) { - if (fitness > 0) max_fitness_ = fitness; + if (fitness > 0) + max_fitness_ = fitness; } float -pcl::ihs::ICP::getMaxFitness () const +pcl::ihs::ICP::getMaxFitness() const { return (max_fitness_); } @@ -125,13 +126,13 @@ pcl::ihs::ICP::getMaxFitness () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::ICP::setCorrespondenceRejectionFactor (const float factor) +pcl::ihs::ICP::setCorrespondenceRejectionFactor(const float factor) { factor_ = factor < 1.f ? 1.f : factor; } float -pcl::ihs::ICP::getCorrespondenceRejectionFactor () const +pcl::ihs::ICP::getCorrespondenceRejectionFactor() const { return (factor_); } @@ -139,13 +140,13 @@ pcl::ihs::ICP::getCorrespondenceRejectionFactor () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::ICP::setMaxAngle (const float angle) +pcl::ihs::ICP::setMaxAngle(const float angle) { - max_angle_ = pcl::ihs::clamp (angle, 0.f, 180.f); + max_angle_ = pcl::ihs::clamp(angle, 0.f, 180.f); } float -pcl::ihs::ICP::getMaxAngle () const +pcl::ihs::ICP::getMaxAngle() const { return (max_angle_); } @@ -153,17 +154,16 @@ pcl::ihs::ICP::getMaxAngle () const //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model, - const CloudXYZRGBNormalConstPtr& cloud_data, - const Eigen::Matrix4f& T_init, - Eigen::Matrix4f& T_final) +pcl::ihs::ICP::findTransformation(const MeshConstPtr& mesh_model, + const CloudXYZRGBNormalConstPtr& cloud_data, + const Eigen::Matrix4f& T_init, + Eigen::Matrix4f& T_final) { // Check the input // TODO: Double check the minimum number of points necessary for icp const std::size_t n_min = 4; - if(mesh_model->sizeVertices () < n_min || cloud_data->size () < n_min) - { + if (mesh_model->sizeVertices() < n_min || cloud_data->size() < n_min) { std::cerr << "ERROR in icp.cpp: Not enough input points!\n"; return (false); } @@ -171,15 +171,15 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model, // Time measurements pcl::StopWatch sw; pcl::StopWatch sw_total; - double t_select = 0.; - double t_build = 0.; - double t_nn_search = 0.; + double t_select = 0.; + double t_build = 0.; + double t_nn_search = 0.; double t_calc_trafo = 0.; // Convergence and registration failure - float current_fitness = 0.f; - float delta_fitness = std::numeric_limits ::max (); - float overlap = std::numeric_limits ::quiet_NaN (); + float current_fitness = 0.f; + float delta_fitness = std::numeric_limits::max(); + float overlap = std::numeric_limits::quiet_NaN(); // Outlier rejection float squared_distance_threshold = std::numeric_limits::max(); @@ -188,204 +188,207 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model, Eigen::Matrix4f T_cur = T_init; // Point selection - sw.reset (); - const CloudNormalConstPtr cloud_model_selected = this->selectModelPoints (mesh_model, T_init.inverse ()); - const CloudNormalConstPtr cloud_data_selected = this->selectDataPoints (cloud_data); - t_select = sw.getTime (); - - const std::size_t n_model = cloud_model_selected->size (); - const std::size_t n_data = cloud_data_selected->size (); - if(n_model < n_min) {std::cerr << "ERROR in icp.cpp: Not enough model points after selection!\n"; return (false);} - if(n_data < n_min) {std::cerr << "ERROR in icp.cpp: Not enough data points after selection!\n"; return (false);} + sw.reset(); + const CloudNormalConstPtr cloud_model_selected = + this->selectModelPoints(mesh_model, T_init.inverse()); + const CloudNormalConstPtr cloud_data_selected = this->selectDataPoints(cloud_data); + t_select = sw.getTime(); + + const std::size_t n_model = cloud_model_selected->size(); + const std::size_t n_data = cloud_data_selected->size(); + if (n_model < n_min) { + std::cerr << "ERROR in icp.cpp: Not enough model points after selection!\n"; + return (false); + } + if (n_data < n_min) { + std::cerr << "ERROR in icp.cpp: Not enough data points after selection!\n"; + return (false); + } // Build a kd-tree - sw.reset (); - kd_tree_->setInputCloud (cloud_model_selected); - t_build = sw.getTime (); + sw.reset(); + kd_tree_->setInputCloud(cloud_model_selected); + t_build = sw.getTime(); - pcl::Indices index (1); - std::vector squared_distance (1); + pcl::Indices index(1); + std::vector squared_distance(1); // Clouds with one to one correspondences CloudNormal cloud_model_corr; CloudNormal cloud_data_corr; - cloud_model_corr.reserve (n_data); - cloud_data_corr.reserve (n_data); + cloud_model_corr.reserve(n_data); + cloud_data_corr.reserve(n_data); // ICP main loop unsigned int iter = 1; PointNormal pt_d; - const float dot_min = std::cos (max_angle_ * 17.45329252e-3); // deg to rad - while (true) - { + const float dot_min = std::cos(max_angle_ * 17.45329252e-3); // deg to rad + while (true) { // Accumulated error float squared_distance_sum = 0.f; // NN search - cloud_model_corr.clear (); - cloud_data_corr.clear (); - sw.reset (); - for (CloudNormal::const_iterator it_d = cloud_data_selected->begin (); it_d!=cloud_data_selected->end (); ++it_d) - { + cloud_model_corr.clear(); + cloud_data_corr.clear(); + sw.reset(); + for (CloudNormal::const_iterator it_d = cloud_data_selected->begin(); + it_d != cloud_data_selected->end(); + ++it_d) { // Transform the data point pt_d = *it_d; - pt_d.getVector4fMap () = T_cur * pt_d.getVector4fMap (); - pt_d.getNormalVector4fMap () = T_cur * pt_d.getNormalVector4fMap (); + pt_d.getVector4fMap() = T_cur * pt_d.getVector4fMap(); + pt_d.getNormalVector4fMap() = T_cur * pt_d.getNormalVector4fMap(); // Find the correspondence to the model points - if (!kd_tree_->nearestKSearch (pt_d, 1, index, squared_distance)) - { + if (!kd_tree_->nearestKSearch(pt_d, 1, index, squared_distance)) { std::cerr << "ERROR in icp.cpp: nearestKSearch failed!\n"; return (false); } // Check the distance threshold - if (squared_distance [0] < squared_distance_threshold) - { - if ((std::size_t) index [0] >= cloud_model_selected->size ()) - { + if (squared_distance[0] < squared_distance_threshold) { + if ((std::size_t)index[0] >= cloud_model_selected->size()) { std::cerr << "ERROR in icp.cpp: Segfault!\n"; - std::cerr << " Trying to access index " << index [0] << " >= " << cloud_model_selected->size () << std::endl; - exit (EXIT_FAILURE); + std::cerr << " Trying to access index " << index[0] + << " >= " << cloud_model_selected->size() << std::endl; + exit(EXIT_FAILURE); } - const PointNormal& pt_m = cloud_model_selected->operator [] (index [0]); + const PointNormal& pt_m = cloud_model_selected->operator[](index[0]); // Check the normals threshold - if (pt_m.getNormalVector4fMap ().dot (pt_d.getNormalVector4fMap ()) > dot_min) - { - squared_distance_sum += squared_distance [0]; + if (pt_m.getNormalVector4fMap().dot(pt_d.getNormalVector4fMap()) > dot_min) { + squared_distance_sum += squared_distance[0]; - cloud_model_corr.push_back (pt_m); - cloud_data_corr.push_back (pt_d); + cloud_model_corr.push_back(pt_m); + cloud_data_corr.push_back(pt_d); } } } - t_nn_search += sw.getTime (); + t_nn_search += sw.getTime(); - const std::size_t n_corr = cloud_data_corr.size (); - if (n_corr < n_min) - { - std::cerr << "ERROR in icp.cpp: Not enough correspondences: " << n_corr << " < " << n_min << std::endl; + const std::size_t n_corr = cloud_data_corr.size(); + if (n_corr < n_min) { + std::cerr << "ERROR in icp.cpp: Not enough correspondences: " << n_corr << " < " + << n_min << std::endl; return (false); } - // NOTE: The fitness is calculated with the transformation from the previous iteration (I don't re-calculate it after the transformation estimation). This means that the actual fitness will be one iteration "better" than the calculated fitness suggests. This should be no problem because the difference is small at the state of convergence. + // NOTE: The fitness is calculated with the transformation from the previous + // iteration (I don't re-calculate it after the transformation estimation). This + // means that the actual fitness will be one iteration "better" than the calculated + // fitness suggests. This should be no problem because the difference is small at + // the state of convergence. float previous_fitness = current_fitness; - current_fitness = squared_distance_sum / static_cast (n_corr); - delta_fitness = std::abs (previous_fitness - current_fitness); + current_fitness = squared_distance_sum / static_cast(n_corr); + delta_fitness = std::abs(previous_fitness - current_fitness); squared_distance_threshold = factor_ * current_fitness; - overlap = static_cast (n_corr) / static_cast (n_data); + overlap = static_cast(n_corr) / static_cast(n_data); // std::cerr << "Iter: " << std::left << std::setw(3) << iter // << " | Overlap: " << std::setprecision(2) << std::setw(4) << overlap - // << " | current fitness: " << std::setprecision(5) << std::setw(10) << current_fitness - // << " | delta fitness: " << std::setprecision(5) << std::setw(10) << delta_fitness << std::endl; + // << " | current fitness: " << std::setprecision(5) << std::setw(10) + // << current_fitness + // << " | delta fitness: " << std::setprecision(5) << std::setw(10) << + // delta_fitness << std::endl; // Minimize the point to plane distance - sw.reset (); - Eigen::Matrix4f T_delta = Eigen::Matrix4f::Identity (); - if (!this->minimizePointPlane (cloud_data_corr, cloud_model_corr, T_delta)) - { + sw.reset(); + Eigen::Matrix4f T_delta = Eigen::Matrix4f::Identity(); + if (!this->minimizePointPlane(cloud_data_corr, cloud_model_corr, T_delta)) { std::cerr << "ERROR in icp.cpp: minimizePointPlane failed!\n"; return (false); } - t_calc_trafo += sw.getTime (); + t_calc_trafo += sw.getTime(); T_cur = T_delta * T_cur; // Convergence - if (delta_fitness < epsilon_) break; + if (delta_fitness < epsilon_) + break; ++iter; - if (iter > max_iterations_) break; + if (iter > max_iterations_) + break; } // End ICP main loop // Some output std::cerr << "Registration:\n" - << " - num model / num data : " - << std::setw (8) << std::right << n_model << " / " - << std::setw (8) << std::left << n_data << "\n" + << " - num model / num data : " << std::setw(8) << std::right + << n_model << " / " << std::setw(8) << std::left << n_data << "\n" - << std::scientific << std::setprecision (1) + << std::scientific << std::setprecision(1) - << " - delta fitness / epsilon : " - << std::setw (8) << std::right << delta_fitness << " / " - << std::setw (8) << std::left << epsilon_ + << " - delta fitness / epsilon : " << std::setw(8) << std::right + << delta_fitness << " / " << std::setw(8) << std::left << epsilon_ << (delta_fitness < epsilon_ ? " <-- :-)\n" : "\n") - << " - fitness / max fitness : " - << std::setw (8) << std::right << current_fitness << " / " - << std::setw (8) << std::left << max_fitness_ + << " - fitness / max fitness : " << std::setw(8) << std::right + << current_fitness << " / " << std::setw(8) << std::left << max_fitness_ << (current_fitness > max_fitness_ ? " <-- :-(\n" : "\n") - << std::fixed << std::setprecision (2) + << std::fixed << std::setprecision(2) - << " - iter / max iter : " - << std::setw (8) << std::right << iter << " / " - << std::setw (8) << std::left << max_iterations_ + << " - iter / max iter : " << std::setw(8) << std::right + << iter << " / " << std::setw(8) << std::left << max_iterations_ << (iter > max_iterations_ ? " <-- :-(\n" : "\n") - << " - overlap / min overlap : " - << std::setw (8) << std::right << overlap << " / " - << std::setw (8) << std::left << min_overlap_ + << " - overlap / min overlap : " << std::setw(8) << std::right + << overlap << " / " << std::setw(8) << std::left << min_overlap_ << (overlap < min_overlap_ ? " <-- :-(\n" : "\n") - << std::fixed << std::setprecision (0) + << std::fixed << std::setprecision(0) - << " - time select : " - << std::setw (8) << std::right << t_select << " ms\n" + << " - time select : " << std::setw(8) << std::right + << t_select << " ms\n" - << " - time build kd-tree : " - << std::setw (8) << std::right << t_build << " ms\n" + << " - time build kd-tree : " << std::setw(8) << std::right + << t_build << " ms\n" - << " - time nn-search / trafo / reject: " - << std::setw (8) << std::right << t_nn_search << " ms\n" + << " - time nn-search / trafo / reject: " << std::setw(8) << std::right + << t_nn_search << " ms\n" - << " - time minimize : " - << std::setw (8) << std::right << t_calc_trafo << " ms\n" + << " - time minimize : " << std::setw(8) << std::right + << t_calc_trafo << " ms\n" - << " - total time : " - << std::setw (8) << std::right << sw_total.getTime () << " ms\n"; + << " - total time : " << std::setw(8) << std::right + << sw_total.getTime() << " ms\n"; - if (iter > max_iterations_ || overlap < min_overlap_ || current_fitness > max_fitness_) - { + if (iter > max_iterations_ || overlap < min_overlap_ || + current_fitness > max_fitness_) { return (false); } - if (delta_fitness <= epsilon_) - { + if (delta_fitness <= epsilon_) { T_final = T_cur; return (true); } std::cerr << "ERROR in icp.cpp: Congratulations! you found a bug.\n"; - exit (EXIT_FAILURE); + exit(EXIT_FAILURE); } //////////////////////////////////////////////////////////////////////////////// pcl::ihs::ICP::CloudNormalConstPtr -pcl::ihs::ICP::selectModelPoints (const MeshConstPtr& mesh_model, - const Eigen::Matrix4f& T_inv) const +pcl::ihs::ICP::selectModelPoints(const MeshConstPtr& mesh_model, + const Eigen::Matrix4f& T_inv) const { - const CloudNormalPtr cloud_model_out (new CloudNormal ()); - cloud_model_out->reserve (mesh_model->sizeVertices ()); + const CloudNormalPtr cloud_model_out(new CloudNormal()); + cloud_model_out->reserve(mesh_model->sizeVertices()); - const Mesh::VertexDataCloud& cloud = mesh_model->getVertexDataCloud (); + const Mesh::VertexDataCloud& cloud = mesh_model->getVertexDataCloud(); - for (const auto &vertex_data : cloud) - { + for (const auto& vertex_data : cloud) { // Don't consider points that are facing away from the camera. - if ((T_inv.lazyProduct (vertex_data.getNormalVector4fMap ())).z () < 0.f) - { + if ((T_inv.lazyProduct(vertex_data.getNormalVector4fMap())).z() < 0.f) { PointNormal pt; - pt.getVector4fMap () = vertex_data.getVector4fMap (); - pt.getNormalVector4fMap () = vertex_data.getNormalVector4fMap (); + pt.getVector4fMap() = vertex_data.getVector4fMap(); + pt.getNormalVector4fMap() = vertex_data.getNormalVector4fMap(); // NOTE: Not the transformed points!! - cloud_model_out->push_back (pt); + cloud_model_out->push_back(pt); } } @@ -395,20 +398,18 @@ pcl::ihs::ICP::selectModelPoints (const MeshConstPtr& mesh_model, //////////////////////////////////////////////////////////////////////////////// pcl::ihs::ICP::CloudNormalConstPtr -pcl::ihs::ICP::selectDataPoints (const CloudXYZRGBNormalConstPtr& cloud_data) const +pcl::ihs::ICP::selectDataPoints(const CloudXYZRGBNormalConstPtr& cloud_data) const { - const CloudNormalPtr cloud_data_out (new CloudNormal ()); - cloud_data_out->reserve (cloud_data->size ()); + const CloudNormalPtr cloud_data_out(new CloudNormal()); + cloud_data_out->reserve(cloud_data->size()); - for (const auto &vertex_data : *cloud_data) - { - if (!std::isnan (vertex_data.x)) - { + for (const auto& vertex_data : *cloud_data) { + if (!std::isnan(vertex_data.x)) { PointNormal pt; - pt.getVector4fMap () = vertex_data.getVector4fMap (); - pt.getNormalVector4fMap () = vertex_data.getNormalVector4fMap (); + pt.getVector4fMap() = vertex_data.getVector4fMap(); + pt.getNormalVector4fMap() = vertex_data.getNormalVector4fMap(); - cloud_data_out->push_back (pt); + cloud_data_out->push_back(pt); } } @@ -418,144 +419,141 @@ pcl::ihs::ICP::selectDataPoints (const CloudXYZRGBNormalConstPtr& cloud_data) co //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::ICP::minimizePointPlane (const CloudNormal& cloud_source, - const CloudNormal& cloud_target, - Eigen::Matrix4f& T) const +pcl::ihs::ICP::minimizePointPlane(const CloudNormal& cloud_source, + const CloudNormal& cloud_target, + Eigen::Matrix4f& T) const { // Check the input // n < n_min already checked in the icp main loop - const std::size_t n = cloud_source.size (); - if (cloud_target.size () != n) - { + const std::size_t n = cloud_source.size(); + if (cloud_target.size() != n) { std::cerr << "ERROR in icp.cpp: Input must have the same size!\n"; return (false); } // For numerical stability - // - Low: Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration (2004), in the discussion: "To improve the numerical stability of the computation, it is important to use a unit of distance that is comparable in magnitude with the rotation angles. The simplest way is to rescale and move the two input surfaces so that they are bounded within a unit sphere or cube centered at the origin." - // - Gelfand et al.: Geometrically Stable Sampling for the ICP Algorithm (2003), in sec 3.1: "As is common with PCA methods, we will shift the center of mass of the points to the origin." ... "Therefore, af- ter shifting the center of mass, we will scale the point set so that the average distance of points from the origin is 1." - // - Hartley, Zisserman: - Multiple View Geometry (2004), page 109: They normalize to sqrt(2) + // - Low: Linear Least-Squares Optimization for Point-to-Plane ICP Surface + // Registration (2004), in the discussion: "To improve the numerical stability of the + // computation, it is important to use a unit of distance that is comparable in + // magnitude with the rotation angles. The simplest way is to rescale and move the two + // input surfaces so that they are bounded within a unit sphere or cube centered at + // the origin." + // - Gelfand et al.: Geometrically Stable Sampling for the ICP Algorithm (2003), in + // sec 3.1: "As is common with PCA methods, we will shift the center of mass of the + // points to the origin." ... "Therefore, af- ter shifting the center of mass, we will + // scale the point set so that the average distance of points from the origin is 1." + // - Hartley, Zisserman: - Multiple View Geometry (2004), page 109: They normalize to + // sqrt(2) // TODO: Check the resulting C matrix for the conditioning. // Subtract the centroid and calculate the scaling factor - Eigen::Vector4f c_s (0.f, 0.f, 0.f, 1.f); - Eigen::Vector4f c_t (0.f, 0.f, 0.f, 1.f); - pcl::compute3DCentroid (cloud_source, c_s); c_s.w () = 1.f; - pcl::compute3DCentroid (cloud_target, c_t); c_t.w () = 1.f; + Eigen::Vector4f c_s(0.f, 0.f, 0.f, 1.f); + Eigen::Vector4f c_t(0.f, 0.f, 0.f, 1.f); + pcl::compute3DCentroid(cloud_source, c_s); + c_s.w() = 1.f; + pcl::compute3DCentroid(cloud_target, c_t); + c_t.w() = 1.f; // The normals are only needed for the target - using Vec4Xf = std::vector >; + using Vec4Xf = + std::vector>; Vec4Xf xyz_s, xyz_t, nor_t; - xyz_s.reserve (n); - xyz_t.reserve (n); - nor_t.reserve (n); + xyz_s.reserve(n); + xyz_t.reserve(n); + nor_t.reserve(n); - CloudNormal::const_iterator it_s = cloud_source.begin (); - CloudNormal::const_iterator it_t = cloud_target.begin (); + CloudNormal::const_iterator it_s = cloud_source.begin(); + CloudNormal::const_iterator it_t = cloud_target.begin(); float accum = 0.f; Eigen::Vector4f pt_s, pt_t; - for (; it_s!=cloud_source.end (); ++it_s, ++it_t) - { + for (; it_s != cloud_source.end(); ++it_s, ++it_t) { // Subtract the centroid - pt_s = it_s->getVector4fMap () - c_s; - pt_t = it_t->getVector4fMap () - c_t; + pt_s = it_s->getVector4fMap() - c_s; + pt_t = it_t->getVector4fMap() - c_t; - xyz_s.push_back (pt_s); - xyz_t.push_back (pt_t); - nor_t.push_back (it_t->getNormalVector4fMap ()); + xyz_s.push_back(pt_s); + xyz_t.push_back(pt_t); + nor_t.push_back(it_t->getNormalVector4fMap()); - // Calculate the radius (L2 norm) of the bounding sphere through both shapes and accumulate the average + // Calculate the radius (L2 norm) of the bounding sphere through both shapes and + // accumulate the average // TODO: Change to squared norm and adapt the rest accordingly - accum += pt_s.head <3> ().norm () + pt_t.head <3> ().norm (); + accum += pt_s.head<3>().norm() + pt_t.head<3>().norm(); } // Inverse factor (do a multiplication instead of division later) - const float factor = 2.f * static_cast (n) / accum; - const float factor_squared = factor*factor; + const float factor = 2.f * static_cast(n) / accum; + const float factor_squared = factor * factor; // Covariance matrix C - Eigen::Matrix C; + Eigen::Matrix C; // Right hand side vector b - Eigen::Matrix b; + Eigen::Matrix b; // For Eigen vectorization: use 4x4 submatrixes instead of 3x3 submatrixes // -> top left 3x3 matrix will form the final C // Same for b - Eigen::Matrix4f C_tl = Eigen::Matrix4f::Zero(); // top left corner + Eigen::Matrix4f C_tl = Eigen::Matrix4f::Zero(); // top left corner Eigen::Matrix4f C_tr_bl = Eigen::Matrix4f::Zero(); // top right / bottom left - Eigen::Matrix4f C_br = Eigen::Matrix4f::Zero(); // bottom right + Eigen::Matrix4f C_br = Eigen::Matrix4f::Zero(); // bottom right - Eigen::Vector4f b_t = Eigen::Vector4f::Zero(); // top - Eigen::Vector4f b_b = Eigen::Vector4f::Zero(); // bottom + Eigen::Vector4f b_t = Eigen::Vector4f::Zero(); // top + Eigen::Vector4f b_b = Eigen::Vector4f::Zero(); // bottom - Vec4Xf::const_iterator it_xyz_s = xyz_s.begin (); - Vec4Xf::const_iterator it_xyz_t = xyz_t.begin (); - Vec4Xf::const_iterator it_nor_t = nor_t.begin (); + Vec4Xf::const_iterator it_xyz_s = xyz_s.begin(); + Vec4Xf::const_iterator it_xyz_t = xyz_t.begin(); + Vec4Xf::const_iterator it_nor_t = nor_t.begin(); Eigen::Vector4f cross; - for (; it_xyz_s!=xyz_s.end (); ++it_xyz_s, ++it_xyz_t, ++it_nor_t) - { - cross = it_xyz_s->cross3 (*it_nor_t); + for (; it_xyz_s != xyz_s.end(); ++it_xyz_s, ++it_xyz_t, ++it_nor_t) { + cross = it_xyz_s->cross3(*it_nor_t); - C_tl += cross * cross. transpose (); - C_tr_bl += cross * it_nor_t->transpose (); - C_br += *it_nor_t * it_nor_t->transpose (); + C_tl += cross * cross.transpose(); + C_tr_bl += cross * it_nor_t->transpose(); + C_br += *it_nor_t * it_nor_t->transpose(); - float dot = (*it_xyz_t-*it_xyz_s).dot (*it_nor_t); + float dot = (*it_xyz_t - *it_xyz_s).dot(*it_nor_t); - b_t += cross * dot; - b_b += *it_nor_t * dot; + b_t += cross * dot; + b_b += *it_nor_t * dot; } // Scale with the factor and copy the 3x3 submatrixes into C and b - C_tl *= factor_squared; + C_tl *= factor_squared; C_tr_bl *= factor; - C << C_tl. topLeftCorner <3, 3> () , C_tr_bl.topLeftCorner <3, 3> (), - C_tr_bl.topLeftCorner <3, 3> ().transpose(), C_br. topLeftCorner <3, 3> (); + C << C_tl.topLeftCorner<3, 3>(), C_tr_bl.topLeftCorner<3, 3>(), + C_tr_bl.topLeftCorner<3, 3>().transpose(), C_br.topLeftCorner<3, 3>(); - b << b_t.head <3> () * factor_squared, - b_b. head <3> () * factor; + b << b_t.head<3>() * factor_squared, b_b.head<3>() * factor; // Solve C * x = b with a Cholesky factorization with pivoting // x = [alpha; beta; gamma; trans_x; trans_y; trans_z] - Eigen::Matrix x = C.selfadjointView ().ldlt ().solve (b); + Eigen::Matrix x = C.selfadjointView().ldlt().solve(b); // The calculated transformation in the scaled coordinate system - const float - sa = std::sin (x (0)), - ca = std::cos (x (0)), - sb = std::sin (x (1)), - cb = std::cos (x (1)), - sg = std::sin (x (2)), - cg = std::cos (x (2)), - tx = x (3), - ty = x (4), - tz = x (5); + const float sa = std::sin(x(0)), ca = std::cos(x(0)), sb = std::sin(x(1)), + cb = std::cos(x(1)), sg = std::sin(x(2)), cg = std::cos(x(2)), tx = x(3), + ty = x(4), tz = x(5); Eigen::Matrix4f TT; - TT << cg*cb, -sg*ca+cg*sb*sa, sg*sa+cg*sb*ca, tx, - sg*cb , cg*ca+sg*sb*sa, -cg*sa+sg*sb*ca, ty, - -sb , cb*sa , cb*ca , tz, - 0.f , 0.f , 0.f , 1.f; + TT << cg * cb, -sg * ca + cg * sb * sa, sg * sa + cg * sb * ca, tx, sg * cb, + cg * ca + sg * sb * sa, -cg * sa + sg * sb * ca, ty, -sb, cb * sa, cb * ca, tz, + 0.f, 0.f, 0.f, 1.f; // Transformation matrixes into the local coordinate systems of model/data Eigen::Matrix4f T_s, T_t; - T_s << factor, 0.f , 0.f , -c_s.x () * factor, - 0.f , factor, 0.f , -c_s.y () * factor, - 0.f , 0.f , factor, -c_s.z () * factor, - 0.f , 0.f , 0.f , 1.f; + T_s << factor, 0.f, 0.f, -c_s.x() * factor, 0.f, factor, 0.f, -c_s.y() * factor, 0.f, + 0.f, factor, -c_s.z() * factor, 0.f, 0.f, 0.f, 1.f; - T_t << factor, 0.f , 0.f , -c_t.x () * factor, - 0.f , factor, 0.f , -c_t.y () * factor, - 0.f , 0.f , factor, -c_t.z () * factor, - 0.f , 0.f , 0.f , 1.f; + T_t << factor, 0.f, 0.f, -c_t.x() * factor, 0.f, factor, 0.f, -c_t.y() * factor, 0.f, + 0.f, factor, -c_t.z() * factor, 0.f, 0.f, 0.f, 1.f; // Output transformation T - T = T_t.inverse () * TT * T_s; + T = T_t.inverse() * TT * T_s; return (true); } diff --git a/apps/in_hand_scanner/src/in_hand_scanner.cpp b/apps/in_hand_scanner/src/in_hand_scanner.cpp index c385c063..95d2744c 100644 --- a/apps/in_hand_scanner/src/in_hand_scanner.cpp +++ b/apps/in_hand_scanner/src/in_hand_scanner.cpp @@ -38,326 +38,355 @@ * */ +#include #include - -#include -#include -#include -#include -#include - -#include +#include +#include +#include #include #include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include +#include + +#include +#include +#include +#include +#include //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::InHandScanner::InHandScanner (Base* parent) - : Base (parent), - running_mode_ (RM_UNPROCESSED), - iteration_ (0), - starting_grabber_ (false), - input_data_processing_ (new InputDataProcessing ()), - icp_ (new ICP ()), - transformation_ (Eigen::Matrix4f::Identity ()), - integration_ (new Integration ()), - mesh_processing_ (new MeshProcessing ()), - mesh_model_ (new Mesh ()), - destructor_called_ (false) +pcl::ihs::InHandScanner::InHandScanner(Base* parent) +: Base(parent) +, running_mode_(RM_UNPROCESSED) +, iteration_(0) +, starting_grabber_(false) +, input_data_processing_(new InputDataProcessing()) +, icp_(new ICP()) +, transformation_(Eigen::Matrix4f::Identity()) +, integration_(new Integration()) +, mesh_processing_(new MeshProcessing()) +, mesh_model_(new Mesh()) +, destructor_called_(false) { // http://doc.qt.digia.com/qt/qmetatype.html#qRegisterMetaType - qRegisterMetaType ("RunningMode"); + qRegisterMetaType("RunningMode"); - Base::setScalingFactor (0.01); + Base::setScalingFactor(0.01); // Initialize the pivot - const float x_min = input_data_processing_->getXMin (); - const float x_max = input_data_processing_->getXMax (); - const float y_min = input_data_processing_->getYMin (); - const float y_max = input_data_processing_->getYMax (); - const float z_min = input_data_processing_->getZMin (); - const float z_max = input_data_processing_->getZMax (); - - Base::setPivot (Eigen::Vector3d ((x_min + x_max) / 2., (y_min + y_max) / 2., (z_min + z_max) / 2.)); + const float x_min = input_data_processing_->getXMin(); + const float x_max = input_data_processing_->getXMax(); + const float y_min = input_data_processing_->getYMin(); + const float y_max = input_data_processing_->getYMax(); + const float z_min = input_data_processing_->getZMin(); + const float z_max = input_data_processing_->getZMax(); + + Base::setPivot(Eigen::Vector3d( + (x_min + x_max) / 2., (y_min + y_max) / 2., (z_min + z_max) / 2.)); } //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::InHandScanner::~InHandScanner () +pcl::ihs::InHandScanner::~InHandScanner() { - std::lock_guard lock (mutex_); + std::lock_guard lock(mutex_); destructor_called_ = true; - if (grabber_ && grabber_->isRunning ()) grabber_->stop (); - if (new_data_connection_.connected ()) new_data_connection_.disconnect (); + if (grabber_ && grabber_->isRunning()) + grabber_->stop(); + if (new_data_connection_.connected()) + new_data_connection_.disconnect(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::startGrabber () +pcl::ihs::InHandScanner::startGrabber() { - QtConcurrent::run ([this] { startGrabberImpl (); }); + QtConcurrent::run([this] { startGrabberImpl(); }); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::showUnprocessedData () +pcl::ihs::InHandScanner::showUnprocessedData() { - std::lock_guard lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; std::cerr << "Showing the unprocessed input data.\n"; - Base::setDrawBox (false); - Base::setColoring (Base::COL_RGB); + Base::setDrawBox(false); + Base::setColoring(Base::COL_RGB); running_mode_ = RM_UNPROCESSED; - emit runningModeChanged (running_mode_); + emit runningModeChanged(running_mode_); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::showProcessedData () +pcl::ihs::InHandScanner::showProcessedData() { - std::lock_guard lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; std::cerr << "Showing the processed input data.\n"; - Base::setDrawBox (true); - Base::setColoring (Base::COL_RGB); + Base::setDrawBox(true); + Base::setColoring(Base::COL_RGB); running_mode_ = RM_PROCESSED; - emit runningModeChanged (running_mode_); + emit runningModeChanged(running_mode_); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::registerContinuously () +pcl::ihs::InHandScanner::registerContinuously() { - std::lock_guard lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; std::cerr << "Continuous registration.\n"; - Base::setDrawBox (true); - Base::setColoring (Base::COL_VISCONF); + Base::setDrawBox(true); + Base::setColoring(Base::COL_VISCONF); running_mode_ = RM_REGISTRATION_CONT; - emit runningModeChanged (running_mode_); + emit runningModeChanged(running_mode_); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::registerOnce () +pcl::ihs::InHandScanner::registerOnce() { - std::lock_guard lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; std::cerr << "Single registration.\n"; - Base::setDrawBox (true); + Base::setDrawBox(true); running_mode_ = RM_REGISTRATION_SINGLE; - emit runningModeChanged (running_mode_); + emit runningModeChanged(running_mode_); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::showModel () +pcl::ihs::InHandScanner::showModel() { - std::lock_guard lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; std::cerr << "Show the model\n"; - Base::setDrawBox (false); + Base::setDrawBox(false); running_mode_ = RM_SHOW_MODEL; - emit runningModeChanged (running_mode_); + emit runningModeChanged(running_mode_); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::removeUnfitVertices () +pcl::ihs::InHandScanner::removeUnfitVertices() { - std::unique_lock lock (mutex_); - if (destructor_called_) return; + std::unique_lock lock(mutex_); + if (destructor_called_) + return; std::cerr << "Removing unfit vertices ...\n"; - integration_->removeUnfitVertices (mesh_model_); - if (mesh_model_->emptyVertices ()) - { + integration_->removeUnfitVertices(mesh_model_); + if (mesh_model_->emptyVertices()) { std::cerr << "Mesh got empty -> Reset\n"; - lock.unlock (); - this->reset (); + lock.unlock(); + this->reset(); } - else - { - lock.unlock (); - this->showModel (); + else { + lock.unlock(); + this->showModel(); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::reset () +pcl::ihs::InHandScanner::reset() { - std::unique_lock lock (mutex_); - if (destructor_called_) return; + std::unique_lock lock(mutex_); + if (destructor_called_) + return; std::cerr << "Reset the scanning pipeline.\n"; - mesh_model_->clear (); - Base::removeAllMeshes (); + mesh_model_->clear(); + Base::removeAllMeshes(); - iteration_ = 0; - transformation_ = Eigen::Matrix4f::Identity (); + iteration_ = 0; + transformation_ = Eigen::Matrix4f::Identity(); - lock.unlock (); - this->showUnprocessedData (); + lock.unlock(); + this->showUnprocessedData(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::saveAs (const std::string& filename, const FileType& filetype) +pcl::ihs::InHandScanner::saveAs(const std::string& filename, const FileType& filetype) { - std::lock_guard lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; pcl::PolygonMesh pm; - pcl::geometry::toFaceVertexMesh (*mesh_model_, pm); - - switch (filetype) - { - case FT_PLY: pcl::io::savePLYFile (filename, pm); break; - case FT_VTK: pcl::io::saveVTKFile (filename, pm); break; - default: break; + pcl::geometry::toFaceVertexMesh(*mesh_model_, pm); + + switch (filetype) { + case FT_PLY: + pcl::io::savePLYFile(filename, pm); + break; + case FT_VTK: + pcl::io::saveVTKFile(filename, pm); + break; + default: + break; } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::keyPressEvent (QKeyEvent* event) +pcl::ihs::InHandScanner::keyPressEvent(QKeyEvent* event) { // Don't allow keyboard callbacks while the grabber is starting up. - if (starting_grabber_) return; - if (destructor_called_) return; - - switch (event->key ()) - { - case Qt::Key_1: this->showUnprocessedData (); break; - case Qt::Key_2: this->showProcessedData (); break; - case Qt::Key_3: this->registerContinuously (); break; - case Qt::Key_4: this->registerOnce (); break; - case Qt::Key_5: this->showModel (); break; - case Qt::Key_6: this->removeUnfitVertices (); break; - case Qt::Key_0: this->reset (); break; - case Qt::Key_C: Base::resetCamera (); break; - case Qt::Key_K: Base::toggleColoring (); break; - case Qt::Key_S: Base::toggleMeshRepresentation (); break; - default: break; + if (starting_grabber_) + return; + if (destructor_called_) + return; + + switch (event->key()) { + case Qt::Key_1: + this->showUnprocessedData(); + break; + case Qt::Key_2: + this->showProcessedData(); + break; + case Qt::Key_3: + this->registerContinuously(); + break; + case Qt::Key_4: + this->registerOnce(); + break; + case Qt::Key_5: + this->showModel(); + break; + case Qt::Key_6: + this->removeUnfitVertices(); + break; + case Qt::Key_0: + this->reset(); + break; + case Qt::Key_C: + Base::resetCamera(); + break; + case Qt::Key_K: + Base::toggleColoring(); + break; + case Qt::Key_S: + Base::toggleMeshRepresentation(); + break; + default: + break; } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::newDataCallback (const CloudXYZRGBAConstPtr& cloud_in) +pcl::ihs::InHandScanner::newDataCallback(const CloudXYZRGBAConstPtr& cloud_in) { - Base::calcFPS (computation_fps_); // Must come before the lock! + Base::calcFPS(computation_fps_); // Must come before the lock! - std::unique_lock lock (mutex_); - if (destructor_called_) return; + std::unique_lock lock(mutex_); + if (destructor_called_) + return; pcl::StopWatch sw; // Input data processing CloudXYZRGBNormalPtr cloud_data; CloudXYZRGBNormalPtr cloud_discarded; - if (running_mode_ == RM_SHOW_MODEL) - { - cloud_data = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); + if (running_mode_ == RM_SHOW_MODEL) { + cloud_data = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); } - else if (running_mode_ == RM_UNPROCESSED) - { - if (!input_data_processing_->calculateNormals (cloud_in, cloud_data)) + else if (running_mode_ == RM_UNPROCESSED) { + if (!input_data_processing_->calculateNormals(cloud_in, cloud_data)) return; } - else if (running_mode_ >= RM_PROCESSED) - { - if (!input_data_processing_->segment (cloud_in, cloud_data, cloud_discarded)) + else if (running_mode_ >= RM_PROCESSED) { + if (!input_data_processing_->segment(cloud_in, cloud_data, cloud_discarded)) return; } - double time_input_data_processing = sw.getTime (); + double time_input_data_processing = sw.getTime(); // Registration & integration - if (running_mode_ >= RM_REGISTRATION_CONT) - { + if (running_mode_ >= RM_REGISTRATION_CONT) { std::cerr << "\nGlobal iteration " << iteration_ << "\n"; std::cerr << "Input data processing:\n" - << " - time : " - << std::setw (8) << std::right << time_input_data_processing << " ms\n"; + << " - time : " << std::setw(8) << std::right + << time_input_data_processing << " ms\n"; - if (iteration_ == 0) - { - transformation_ = Eigen::Matrix4f::Identity (); + if (iteration_ == 0) { + transformation_ = Eigen::Matrix4f::Identity(); - sw.reset (); - integration_->reconstructMesh (cloud_data, mesh_model_); + sw.reset(); + integration_->reconstructMesh(cloud_data, mesh_model_); std::cerr << "Integration:\n" - << " - time reconstruct mesh : " - << std::setw (8) << std::right << sw.getTime () << " ms\n"; + << " - time reconstruct mesh : " << std::setw(8) << std::right + << sw.getTime() << " ms\n"; - cloud_data = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); + cloud_data = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); ++iteration_; } - else - { - Eigen::Matrix4f T_final = Eigen::Matrix4f::Identity (); - if (icp_->findTransformation (mesh_model_, cloud_data, transformation_, T_final)) - { + else { + Eigen::Matrix4f T_final = Eigen::Matrix4f::Identity(); + if (icp_->findTransformation(mesh_model_, cloud_data, transformation_, T_final)) { transformation_ = T_final; - sw.reset (); - integration_->merge (cloud_data, mesh_model_, transformation_); + sw.reset(); + integration_->merge(cloud_data, mesh_model_, transformation_); std::cerr << "Integration:\n" - << " - time merge : " - << std::setw (8) << std::right << sw.getTime () << " ms\n"; - - sw.reset (); - integration_->age (mesh_model_); - std::cerr << " - time age : " - << std::setw (8) << std::right << sw.getTime () << " ms\n"; - - sw.reset (); - std::vector boundary_collection; - pcl::geometry::getBoundBoundaryHalfEdges (*mesh_model_, boundary_collection, 1000); - std::cerr << " - time compute boundary : " - << std::setw (8) << std::right << sw.getTime () << " ms\n"; - - sw.reset (); - mesh_processing_->processBoundary (*mesh_model_, boundary_collection); - std::cerr << " - time mesh processing : " - << std::setw (8) << std::right << sw.getTime () << " ms\n"; - - cloud_data = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); + << " - time merge : " << std::setw(8) + << std::right << sw.getTime() << " ms\n"; + + sw.reset(); + integration_->age(mesh_model_); + std::cerr << " - time age : " << std::setw(8) + << std::right << sw.getTime() << " ms\n"; + + sw.reset(); + std::vector boundary_collection; + pcl::geometry::getBoundBoundaryHalfEdges( + *mesh_model_, boundary_collection, 1000); + std::cerr << " - time compute boundary : " << std::setw(8) + << std::right << sw.getTime() << " ms\n"; + + sw.reset(); + mesh_processing_->processBoundary(*mesh_model_, boundary_collection); + std::cerr << " - time mesh processing : " << std::setw(8) + << std::right << sw.getTime() << " ms\n"; + + cloud_data = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); ++iteration_; } } @@ -365,125 +394,132 @@ pcl::ihs::InHandScanner::newDataCallback (const CloudXYZRGBAConstPtr& cloud_in) // Visualization & copy back some variables double time_model = 0; - double time_data = 0; + double time_data = 0; - if (mesh_model_->empty ()) Base::setPivot ("data"); - else Base::setPivot ("model"); + if (mesh_model_->empty()) + Base::setPivot("data"); + else + Base::setPivot("model"); - sw.reset (); - Base::addMesh (mesh_model_, "model", Eigen::Isometry3d (transformation_.inverse ().cast ())); - time_model = sw.getTime (); + sw.reset(); + Base::addMesh(mesh_model_, + "model", + Eigen::Isometry3d(transformation_.inverse().cast())); + time_model = sw.getTime(); - sw.reset (); - Base::addMesh (cloud_data , "data"); // Converts to a mesh for visualization + sw.reset(); + Base::addMesh(cloud_data, "data"); // Converts to a mesh for visualization - if (running_mode_ < RM_REGISTRATION_CONT && cloud_discarded) - { - Base::addMesh (cloud_discarded, "cloud_discarded"); + if (running_mode_ < RM_REGISTRATION_CONT && cloud_discarded) { + Base::addMesh(cloud_discarded, "cloud_discarded"); } - else - { - Base::removeMesh ("cloud_discarded"); + else { + Base::removeMesh("cloud_discarded"); } - time_data = sw.getTime (); + time_data = sw.getTime(); - if (running_mode_ >= RM_REGISTRATION_CONT) - { + if (running_mode_ >= RM_REGISTRATION_CONT) { std::cerr << "Copy to visualization thread:\n" - << " - time model : " - << std::setw (8) << std::right << time_model << " ms\n" - << " - time data : " - << std::setw (8) << std::right << time_data << " ms\n"; + << " - time model : " << std::setw(8) << std::right + << time_model << " ms\n" + << " - time data : " << std::setw(8) << std::right + << time_data << " ms\n"; } - if (running_mode_ == RM_REGISTRATION_SINGLE) - { - lock.unlock (); - this->showProcessedData (); + if (running_mode_ == RM_REGISTRATION_SINGLE) { + lock.unlock(); + this->showProcessedData(); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::paintEvent (QPaintEvent* event) +pcl::ihs::InHandScanner::paintEvent(QPaintEvent* event) { - // std::lock_guard lock (mutex_); - if (destructor_called_) return; - - Base::calcFPS (visualization_fps_); - Base::BoxCoefficients coeffs (input_data_processing_->getXMin (), - input_data_processing_->getXMax (), - input_data_processing_->getYMin (), - input_data_processing_->getYMax (), - input_data_processing_->getZMin (), - input_data_processing_->getZMax (), - Eigen::Isometry3d::Identity ()); - Base::setBoxCoefficients (coeffs); - - Base::setVisibilityConfidenceNormalization (static_cast (integration_->getMinDirections ())); - // lock.unlock (); - - Base::paintEvent (event); - this->drawText (); // NOTE: Must come AFTER the opengl calls + if (destructor_called_) + return; + + Base::calcFPS(visualization_fps_); + Base::BoxCoefficients coeffs(input_data_processing_->getXMin(), + input_data_processing_->getXMax(), + input_data_processing_->getYMin(), + input_data_processing_->getYMax(), + input_data_processing_->getZMin(), + input_data_processing_->getZMax(), + Eigen::Isometry3d::Identity()); + Base::setBoxCoefficients(coeffs); + + Base::setVisibilityConfidenceNormalization( + static_cast(integration_->getMinDirections())); + + Base::paintEvent(event); + this->drawText(); // NOTE: Must come AFTER the opengl calls } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::drawText () +pcl::ihs::InHandScanner::drawText() { - QPainter painter (this); - painter.setPen (Qt::white); + QPainter painter(this); + painter.setPen(Qt::white); QFont font; - if (starting_grabber_) - { - font.setPointSize (this->width () / 20); - painter.setFont (font); - painter.drawText (0, 0, this->width (), this->height (), Qt::AlignHCenter | Qt::AlignVCenter, "Starting the grabber.\n Please wait."); + if (starting_grabber_) { + font.setPointSize(this->width() / 20); + painter.setFont(font); + painter.drawText(0, + 0, + this->width(), + this->height(), + Qt::AlignHCenter | Qt::AlignVCenter, + "Starting the grabber.\n Please wait."); } - else - { - std::string vis_fps ("Visualization: "), comp_fps ("Computation: "); + else { + std::string vis_fps("Visualization: "), comp_fps("Computation: "); - vis_fps.append (visualization_fps_.str ()).append (" fps"); - comp_fps.append (computation_fps_.str ()).append (" fps"); + vis_fps.append(visualization_fps_.str()).append(" fps"); + comp_fps.append(computation_fps_.str()).append(" fps"); - const std::string str = std::string (comp_fps).append ("\n").append (vis_fps); + const std::string str = std::string(comp_fps).append("\n").append(vis_fps); - font.setPointSize (this->width () / 50); + font.setPointSize(this->width() / 50); - painter.setFont (font); - painter.drawText (0, 0, this->width (), this->height (), Qt::AlignBottom | Qt::AlignLeft, str.c_str ()); + painter.setFont(font); + painter.drawText(0, + 0, + this->width(), + this->height(), + Qt::AlignBottom | Qt::AlignLeft, + str.c_str()); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InHandScanner::startGrabberImpl () +pcl::ihs::InHandScanner::startGrabberImpl() { - std::unique_lock lock (mutex_); + std::unique_lock lock(mutex_); starting_grabber_ = true; - lock.unlock (); + lock.unlock(); - try - { - grabber_ = GrabberPtr (new Grabber ()); - } - catch (const pcl::PCLException& e) - { - std::cerr << "ERROR in in_hand_scanner.cpp: " << e.what () << std::endl; - exit (EXIT_FAILURE); + try { + grabber_ = GrabberPtr(new Grabber()); + } catch (const pcl::PCLException& e) { + std::cerr << "ERROR in in_hand_scanner.cpp: " << e.what() << std::endl; + exit(EXIT_FAILURE); } - lock.lock (); - if (destructor_called_) return; + lock.lock(); + if (destructor_called_) + return; - std::function new_data_cb = [this] (const CloudXYZRGBAConstPtr& cloud) { newDataCallback (cloud); }; - new_data_connection_ = grabber_->registerCallback (new_data_cb); - grabber_->start (); + std::function new_data_cb = + [this](const CloudXYZRGBAConstPtr& cloud) { newDataCallback(cloud); }; + new_data_connection_ = grabber_->registerCallback(new_data_cb); + grabber_->start(); starting_grabber_ = false; } diff --git a/apps/in_hand_scanner/src/input_data_processing.cpp b/apps/in_hand_scanner/src/input_data_processing.cpp index 22df6626..748d7da5 100644 --- a/apps/in_hand_scanner/src/input_data_processing.cpp +++ b/apps/in_hand_scanner/src/input_data_processing.cpp @@ -39,73 +39,68 @@ */ #include - #include #include //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::InputDataProcessing::InputDataProcessing () - : normal_estimation_ (new NormalEstimation ()), - - x_min_ (-15.f), - x_max_ ( 15.f), - y_min_ (-15.f), - y_max_ ( 15.f), - z_min_ ( 48.f), - z_max_ ( 70.f), - - h_min_ (210.f), - h_max_ (270.f), - s_min_ ( 0.2f), - s_max_ ( 1.f), - v_min_ ( 0.2f), - v_max_ ( 1.f), - - hsv_inverted_ (false), - hsv_enabled_ (true), - - size_dilate_ (3), - size_erode_ (3) +pcl::ihs::InputDataProcessing::InputDataProcessing() +: normal_estimation_(new NormalEstimation()) +, x_min_(-15.f) +, x_max_(15.f) +, y_min_(-15.f) +, y_max_(15.f) +, z_min_(48.f) +, z_max_(70.f) +, h_min_(210.f) +, h_max_(270.f) +, s_min_(0.2f) +, s_max_(1.f) +, v_min_(0.2f) +, v_max_(1.f) +, hsv_inverted_(false) +, hsv_enabled_(true) +, size_dilate_(3) +, size_erode_(3) { // Normal estimation - normal_estimation_->setNormalEstimationMethod (NormalEstimation::AVERAGE_3D_GRADIENT); - normal_estimation_->setMaxDepthChangeFactor (0.02f); // in meters - normal_estimation_->setNormalSmoothingSize (10.0f); + normal_estimation_->setNormalEstimationMethod(NormalEstimation::AVERAGE_3D_GRADIENT); + normal_estimation_->setMaxDepthChangeFactor(0.02f); // in meters + normal_estimation_->setNormalSmoothingSize(10.0f); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::InputDataProcessing::segment (const CloudXYZRGBAConstPtr& cloud_in, - CloudXYZRGBNormalPtr& cloud_out, - CloudXYZRGBNormalPtr& cloud_discarded) const +pcl::ihs::InputDataProcessing::segment(const CloudXYZRGBAConstPtr& cloud_in, + CloudXYZRGBNormalPtr& cloud_out, + CloudXYZRGBNormalPtr& cloud_discarded) const { - if (!cloud_in) - { + if (!cloud_in) { std::cerr << "ERROR in input_data_processing.cpp: Input cloud is invalid.\n"; return (false); } - if (!cloud_in->isOrganized ()) - { + if (!cloud_in->isOrganized()) { std::cerr << "ERROR in input_data_processing.cpp: Input cloud must be organized.\n"; return (false); } - if (!cloud_out) cloud_out = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); - if (!cloud_discarded) cloud_discarded = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); + if (!cloud_out) + cloud_out = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); + if (!cloud_discarded) + cloud_discarded = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); - const unsigned int width = cloud_in->width; + const unsigned int width = cloud_in->width; const unsigned int height = cloud_in->height; // Calculate the normals - CloudNormalsPtr cloud_normals (new CloudNormals ()); - normal_estimation_->setInputCloud (cloud_in); - normal_estimation_->compute (*cloud_normals); + CloudNormalsPtr cloud_normals(new CloudNormals()); + normal_estimation_->setInputCloud(cloud_in); + normal_estimation_->compute(*cloud_normals); // Get the XYZ and HSV masks. - MatrixXb xyz_mask (height, width); - MatrixXb hsv_mask (height, width); + MatrixXb xyz_mask(height, width); + MatrixXb hsv_mask(height, width); // cm -> m for the comparison const float x_min = .01f * x_min_; @@ -116,42 +111,41 @@ pcl::ihs::InputDataProcessing::segment (const CloudXYZRGBAConstPtr& cloud_in, const float z_max = .01f * z_max_; float h, s, v; - for (MatrixXb::Index r=0; r= x_min && xyzrgb.x <= x_max && - xyzrgb.y >= y_min && xyzrgb.y <= y_max && - xyzrgb.z >= z_min && xyzrgb.z <= z_max) - { - xyz_mask (r, c) = true; - - this->RGBToHSV (xyzrgb.r, xyzrgb.g, xyzrgb.b, h, s, v); - if (h >= h_min_ && h <= h_max_ && s >= s_min_ && s <= s_max_ && v >= v_min_ && v <= v_max_) - { - if (!hsv_inverted_) hsv_mask (r, c) = true; + for (MatrixXb::Index r = 0; r < xyz_mask.rows(); ++r) { + for (MatrixXb::Index c = 0; c < xyz_mask.cols(); ++c) { + const PointXYZRGBA& xyzrgb = (*cloud_in)[r * width + c]; + const Normal& normal = (*cloud_normals)[r * width + c]; + + xyz_mask(r, c) = hsv_mask(r, c) = false; + + if (!std::isnan(xyzrgb.x) && !std::isnan(normal.normal_x) && xyzrgb.x >= x_min && + xyzrgb.x <= x_max && xyzrgb.y >= y_min && xyzrgb.y <= y_max && + xyzrgb.z >= z_min && xyzrgb.z <= z_max) { + xyz_mask(r, c) = true; + + this->RGBToHSV(xyzrgb.r, xyzrgb.g, xyzrgb.b, h, s, v); + if (h >= h_min_ && h <= h_max_ && s >= s_min_ && s <= s_max_ && v >= v_min_ && + v <= v_max_) { + if (!hsv_inverted_) + hsv_mask(r, c) = true; } - else - { - if (hsv_inverted_) hsv_mask (r, c) = true; + else { + if (hsv_inverted_) + hsv_mask(r, c) = true; } } } } - this->erode (xyz_mask, size_erode_); - if (hsv_enabled_) this->dilate (hsv_mask, size_dilate_); - else hsv_mask.setZero (); + this->erode(xyz_mask, size_erode_); + if (hsv_enabled_) + this->dilate(hsv_mask, size_dilate_); + else + hsv_mask.setZero(); // Copy the normals into the clouds. - cloud_out->reserve (cloud_in->size ()); - cloud_discarded->reserve (cloud_in->size ()); + cloud_out->reserve(cloud_in->size()); + cloud_discarded->reserve(cloud_in->size()); pcl::PointXYZRGBNormal pt_out, pt_discarded; pt_discarded.r = 50; @@ -159,49 +153,43 @@ pcl::ihs::InputDataProcessing::segment (const CloudXYZRGBAConstPtr& cloud_in, pt_discarded.b = 230; PointXYZRGBA xyzrgb; - Normal normal; + Normal normal; - for (MatrixXb::Index r=0; r cm - xyzrgb.getVector3fMap () = 100.f * xyzrgb.getVector3fMap (); + xyzrgb.getVector3fMap() = 100.f * xyzrgb.getVector3fMap(); - if (hsv_mask (r, c)) - { - pt_discarded.getVector4fMap () = xyzrgb.getVector4fMap (); - pt_discarded.getNormalVector4fMap () = normal.getNormalVector4fMap (); + if (hsv_mask(r, c)) { + pt_discarded.getVector4fMap() = xyzrgb.getVector4fMap(); + pt_discarded.getNormalVector4fMap() = normal.getNormalVector4fMap(); - pt_out.x = std::numeric_limits ::quiet_NaN (); + pt_out.x = std::numeric_limits::quiet_NaN(); } - else - { - pt_out.getVector4fMap () = xyzrgb.getVector4fMap (); - pt_out.getNormalVector4fMap () = normal.getNormalVector4fMap (); - pt_out.rgba = xyzrgb.rgba; + else { + pt_out.getVector4fMap() = xyzrgb.getVector4fMap(); + pt_out.getNormalVector4fMap() = normal.getNormalVector4fMap(); + pt_out.rgba = xyzrgb.rgba; - pt_discarded.x = std::numeric_limits ::quiet_NaN (); + pt_discarded.x = std::numeric_limits::quiet_NaN(); } } - else - { - pt_out.x = std::numeric_limits ::quiet_NaN (); - pt_discarded.x = std::numeric_limits ::quiet_NaN (); + else { + pt_out.x = std::numeric_limits::quiet_NaN(); + pt_discarded.x = std::numeric_limits::quiet_NaN(); } - cloud_out->push_back (pt_out); - cloud_discarded->push_back (pt_discarded); + cloud_out->push_back(pt_out); + cloud_discarded->push_back(pt_discarded); } } - cloud_out->width = cloud_discarded->width = width; - cloud_out->height = cloud_discarded->height = height; + cloud_out->width = cloud_discarded->width = width; + cloud_out->height = cloud_discarded->height = height; cloud_out->is_dense = cloud_discarded->is_dense = false; return (true); @@ -210,50 +198,48 @@ pcl::ihs::InputDataProcessing::segment (const CloudXYZRGBAConstPtr& cloud_in, //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::InputDataProcessing::calculateNormals (const CloudXYZRGBAConstPtr& cloud_in, - CloudXYZRGBNormalPtr& cloud_out) const +pcl::ihs::InputDataProcessing::calculateNormals(const CloudXYZRGBAConstPtr& cloud_in, + CloudXYZRGBNormalPtr& cloud_out) const { - if (!cloud_in) - { + if (!cloud_in) { std::cerr << "ERROR in input_data_processing.cpp: Input cloud is invalid.\n"; return (false); } if (!cloud_out) - cloud_out = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); + cloud_out = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); // Calculate the normals - CloudNormalsPtr cloud_normals (new CloudNormals ()); - normal_estimation_->setInputCloud (cloud_in); - normal_estimation_->compute (*cloud_normals); + CloudNormalsPtr cloud_normals(new CloudNormals()); + normal_estimation_->setInputCloud(cloud_in); + normal_estimation_->compute(*cloud_normals); // Copy the input cloud and normals into the output cloud. - cloud_out->resize (cloud_in->size ()); - cloud_out->width = cloud_in->width; - cloud_out->height = cloud_in->height; + cloud_out->resize(cloud_in->size()); + cloud_out->width = cloud_in->width; + cloud_out->height = cloud_in->height; cloud_out->is_dense = false; - CloudNormals::const_iterator it_n = cloud_normals->begin (); - CloudXYZRGBNormal::iterator it_out = cloud_out->begin (); + CloudNormals::const_iterator it_n = cloud_normals->begin(); + CloudXYZRGBNormal::iterator it_out = cloud_out->begin(); PointXYZRGBNormal invalid_pt; - invalid_pt.x = invalid_pt.y = invalid_pt.z = std::numeric_limits ::quiet_NaN (); - invalid_pt.normal_x = invalid_pt.normal_y = invalid_pt.normal_z = std::numeric_limits ::quiet_NaN (); - invalid_pt.data [3] = 1.f; - invalid_pt.data_n [3] = 0.f; - - for (auto it_in = cloud_in->begin (); it_in!=cloud_in->end (); ++it_in, ++it_n, ++it_out) - { - if (!it_n->getNormalVector4fMap (). hasNaN ()) - { + invalid_pt.x = invalid_pt.y = invalid_pt.z = std::numeric_limits::quiet_NaN(); + invalid_pt.normal_x = invalid_pt.normal_y = invalid_pt.normal_z = + std::numeric_limits::quiet_NaN(); + invalid_pt.data[3] = 1.f; + invalid_pt.data_n[3] = 0.f; + + for (auto it_in = cloud_in->begin(); it_in != cloud_in->end(); + ++it_in, ++it_n, ++it_out) { + if (!it_n->getNormalVector4fMap().hasNaN()) { // m -> cm - it_out->getVector4fMap () = 100.f * it_in->getVector4fMap (); - it_out->data [3] = 1.f; - it_out->rgba = it_in->rgba; - it_out->getNormalVector4fMap () = it_n->getNormalVector4fMap (); + it_out->getVector4fMap() = 100.f * it_in->getVector4fMap(); + it_out->data[3] = 1.f; + it_out->rgba = it_in->rgba; + it_out->getNormalVector4fMap() = it_n->getNormalVector4fMap(); } - else - { + else { *it_out = invalid_pt; } } @@ -264,52 +250,50 @@ pcl::ihs::InputDataProcessing::calculateNormals (const CloudXYZRGBAConstPtr& clo //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InputDataProcessing::erode (MatrixXb& mask, const int k) const +pcl::ihs::InputDataProcessing::erode(MatrixXb& mask, const int k) const { - mask = manhattan (mask, false).array () > k; + mask = manhattan(mask, false).array() > k; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InputDataProcessing::dilate (MatrixXb& mask, const int k) const +pcl::ihs::InputDataProcessing::dilate(MatrixXb& mask, const int k) const { - mask = manhattan (mask, true).array () <= k; + mask = manhattan(mask, true).array() <= k; } //////////////////////////////////////////////////////////////////////////////// pcl::ihs::InputDataProcessing::MatrixXi -pcl::ihs::InputDataProcessing::manhattan (const MatrixXb& mat, const bool comp) const +pcl::ihs::InputDataProcessing::manhattan(const MatrixXb& mat, const bool comp) const { - MatrixXi dist (mat.rows (), mat.cols ()); - const int d_max = dist.rows () + dist.cols (); + MatrixXi dist(mat.rows(), mat.cols()); + const int d_max = dist.rows() + dist.cols(); // Forward - for (MatrixXi::Index r = 0; r < dist.rows (); ++r) - { - for (MatrixXi::Index c = 0; c < dist.cols (); ++c) - { - if (mat (r, c) == comp) - { - dist (r, c) = 0; + for (MatrixXi::Index r = 0; r < dist.rows(); ++r) { + for (MatrixXi::Index c = 0; c < dist.cols(); ++c) { + if (mat(r, c) == comp) { + dist(r, c) = 0; } - else - { - dist (r, c) = d_max; - if (r > 0) dist (r, c) = std::min (dist (r, c), dist (r-1, c ) + 1); - if (c > 0) dist (r, c) = std::min (dist (r, c), dist (r , c-1) + 1); + else { + dist(r, c) = d_max; + if (r > 0) + dist(r, c) = std::min(dist(r, c), dist(r - 1, c) + 1); + if (c > 0) + dist(r, c) = std::min(dist(r, c), dist(r, c - 1) + 1); } } } // Backward - for (int r = dist.rows () - 1; r >= 0; --r) - { - for (int c = dist.cols () - 1; c >= 0; --c) - { - if (r < dist.rows ()-1) dist (r, c) = std::min (dist (r, c), dist (r+1, c ) + 1); - if (c < dist.cols ()-1) dist (r, c) = std::min (dist (r, c), dist (r , c+1) + 1); + for (int r = dist.rows() - 1; r >= 0; --r) { + for (int c = dist.cols() - 1; c >= 0; --c) { + if (r < dist.rows() - 1) + dist(r, c) = std::min(dist(r, c), dist(r + 1, c) + 1); + if (c < dist.cols() - 1) + dist(r, c) = std::min(dist(r, c), dist(r, c + 1) + 1); } } @@ -319,17 +303,17 @@ pcl::ihs::InputDataProcessing::manhattan (const MatrixXb& mat, const bool comp) //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::InputDataProcessing::RGBToHSV (const unsigned char r, - const unsigned char g, - const unsigned char b, - float& h, - float& s, - float& v) const +pcl::ihs::InputDataProcessing::RGBToHSV(const unsigned char r, + const unsigned char g, + const unsigned char b, + float& h, + float& s, + float& v) const { - const unsigned char max = std::max (r, std::max (g, b)); - const unsigned char min = std::min (r, std::min (g, b)); + const unsigned char max = std::max(r, std::max(g, b)); + const unsigned char min = std::min(r, std::min(g, b)); - v = static_cast (max) / 255.f; + v = static_cast(max) / 255.f; if (max == 0) // division by zero { @@ -338,8 +322,8 @@ pcl::ihs::InputDataProcessing::RGBToHSV (const unsigned char r, return; } - const float diff = static_cast (max - min); - s = diff / static_cast (max); + const float diff = static_cast(max - min); + s = diff / static_cast(max); if (min == max) // diff == 0 -> division by zero { @@ -347,11 +331,15 @@ pcl::ihs::InputDataProcessing::RGBToHSV (const unsigned char r, return; } - if (max == r) h = 60.f * ( static_cast (g - b) / diff); - else if (max == g) h = 60.f * (2.f + static_cast (b - r) / diff); - else h = 60.f * (4.f + static_cast (r - g) / diff); // max == b + if (max == r) + h = 60.f * (static_cast(g - b) / diff); + else if (max == g) + h = 60.f * (2.f + static_cast(b - r) / diff); + else + h = 60.f * (4.f + static_cast(r - g) / diff); // max == b - if (h < 0.f) h += 360.f; + if (h < 0.f) + h += 360.f; } //////////////////////////////////////////////////////////////////////////////// diff --git a/apps/in_hand_scanner/src/integration.cpp b/apps/in_hand_scanner/src/integration.cpp index a779afe1..9b5bdc11 100644 --- a/apps/in_hand_scanner/src/integration.cpp +++ b/apps/in_hand_scanner/src/integration.cpp @@ -39,82 +39,78 @@ */ #include +#include +#include +#include #include -#include #include - -#include -#include -#include +#include //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::Integration::Integration () - : kd_tree_ (new pcl::KdTreeFLANN ()), - max_squared_distance_ (0.04f), // 0.2cm - max_angle_ (45.f), - min_weight_ (.3f), - max_age_ (30), - min_directions_ (5) -{ -} +pcl::ihs::Integration::Integration() +: kd_tree_(new pcl::KdTreeFLANN()) +, max_squared_distance_(0.04f) +, // 0.2cm +max_angle_(45.f) +, min_weight_(.3f) +, max_age_(30) +, min_directions_(5) +{} //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::Integration::reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_data, - MeshPtr& mesh_model) const +pcl::ihs::Integration::reconstructMesh(const CloudXYZRGBNormalConstPtr& cloud_data, + MeshPtr& mesh_model) const { - if (!cloud_data) - { + if (!cloud_data) { std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n"; return (false); } - if (!cloud_data->isOrganized ()) - { + if (!cloud_data->isOrganized()) { std::cerr << "ERROR in integration.cpp: Cloud is not organized\n"; return (false); } - const int width = static_cast (cloud_data->width); - const int height = static_cast (cloud_data->height); + const int width = static_cast(cloud_data->width); + const int height = static_cast(cloud_data->height); - if (!mesh_model) mesh_model = MeshPtr (new Mesh ()); + if (!mesh_model) + mesh_model = MeshPtr(new Mesh()); - mesh_model->clear (); - mesh_model->reserveVertices (cloud_data->size ()); - mesh_model->reserveEdges ((width-1) * height + width * (height-1) + (width-1) * (height-1)); - mesh_model->reserveFaces (2 * (width-1) * (height-1)); + mesh_model->clear(); + mesh_model->reserveVertices(cloud_data->size()); + mesh_model->reserveEdges((width - 1) * height + width * (height - 1) + + (width - 1) * (height - 1)); + mesh_model->reserveFaces(2 * (width - 1) * (height - 1)); // Store which vertex is set at which position (initialized with invalid indices) - VertexIndices vertex_indices (cloud_data->size (), VertexIndex ()); + VertexIndices vertex_indices(cloud_data->size(), VertexIndex()); - // Convert to the model cloud type. This is actually not needed but avoids code duplication (see merge). And reconstructMesh is called only the first reconstruction step anyway. - // NOTE: The default constructor of PointIHS has to initialize with NaNs! - CloudIHSPtr cloud_model (new CloudIHS ()); - cloud_model->resize (cloud_data->size ()); + // Convert to the model cloud type. This is actually not needed but avoids code + // duplication (see merge). And reconstructMesh is called only the first + // reconstruction step anyway. NOTE: The default constructor of PointIHS has to + // initialize with NaNs! + CloudIHSPtr cloud_model(new CloudIHS()); + cloud_model->resize(cloud_data->size()); // Set the model points not reached by the main loop - for (int c=0; coperator [] (c); + for (int c = 0; c < width; ++c) { + const PointXYZRGBNormal& pt_d = cloud_data->operator[](c); const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1]) - if (!std::isnan (pt_d.x) && weight > min_weight_) - { - cloud_model->operator [] (c) = PointIHS (pt_d, weight); + if (!std::isnan(pt_d.x) && weight > min_weight_) { + cloud_model->operator[](c) = PointIHS(pt_d, weight); } } - for (int r=1; roperator [] (r*width + c); + for (int r = 1; r < height; ++r) { + for (int c = 0; c < 2; ++c) { + const PointXYZRGBNormal& pt_d = cloud_data->operator[](r * width + c); const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1]) - if (!std::isnan (pt_d.x) && weight > min_weight_) - { - cloud_model->operator [] (r*width + c) = PointIHS (pt_d, weight); + if (!std::isnan(pt_d.x) && weight > min_weight_) { + cloud_model->operator[](r * width + c) = PointIHS(pt_d, weight); } } } @@ -128,49 +124,48 @@ pcl::ihs::Integration::reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_d // * 3 - 0 // const int offset_1 = -width; const int offset_2 = -width - 1; - const int offset_3 = - 1; + const int offset_3 = -1; const int offset_4 = -width - 2; - for (int r=1; r= 0 && ind_0 < static_cast (cloud_data->size ())); - assert (ind_1 >= 0 && ind_1 < static_cast (cloud_data->size ())); - assert (ind_2 >= 0 && ind_2 < static_cast (cloud_data->size ())); - assert (ind_3 >= 0 && ind_3 < static_cast (cloud_data->size ())); - assert (ind_4 >= 0 && ind_4 < static_cast (cloud_data->size ())); - - const PointXYZRGBNormal& pt_d_0 = cloud_data->operator [] (ind_0); - PointIHS& pt_m_0 = cloud_model->operator [] (ind_0); - const PointIHS& pt_m_1 = cloud_model->operator [] (ind_1); - const PointIHS& pt_m_2 = cloud_model->operator [] (ind_2); - const PointIHS& pt_m_3 = cloud_model->operator [] (ind_3); - const PointIHS& pt_m_4 = cloud_model->operator [] (ind_4); - - VertexIndex& vi_0 = vertex_indices [ind_0]; - VertexIndex& vi_1 = vertex_indices [ind_1]; - VertexIndex& vi_2 = vertex_indices [ind_2]; - VertexIndex& vi_3 = vertex_indices [ind_3]; - VertexIndex& vi_4 = vertex_indices [ind_4]; + assert(ind_0 >= 0 && ind_0 < static_cast(cloud_data->size())); + assert(ind_1 >= 0 && ind_1 < static_cast(cloud_data->size())); + assert(ind_2 >= 0 && ind_2 < static_cast(cloud_data->size())); + assert(ind_3 >= 0 && ind_3 < static_cast(cloud_data->size())); + assert(ind_4 >= 0 && ind_4 < static_cast(cloud_data->size())); + + const PointXYZRGBNormal& pt_d_0 = cloud_data->operator[](ind_0); + PointIHS& pt_m_0 = cloud_model->operator[](ind_0); + const PointIHS& pt_m_1 = cloud_model->operator[](ind_1); + const PointIHS& pt_m_2 = cloud_model->operator[](ind_2); + const PointIHS& pt_m_3 = cloud_model->operator[](ind_3); + const PointIHS& pt_m_4 = cloud_model->operator[](ind_4); + + VertexIndex& vi_0 = vertex_indices[ind_0]; + VertexIndex& vi_1 = vertex_indices[ind_1]; + VertexIndex& vi_2 = vertex_indices[ind_2]; + VertexIndex& vi_3 = vertex_indices[ind_3]; + VertexIndex& vi_4 = vertex_indices[ind_4]; const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1]) - if (!std::isnan (pt_d_0.x) && weight > min_weight_) - { - pt_m_0 = PointIHS (pt_d_0, weight); + if (!std::isnan(pt_d_0.x) && weight > min_weight_) { + pt_m_0 = PointIHS(pt_d_0, weight); } - this->addToMesh (pt_m_0,pt_m_1,pt_m_2,pt_m_3, vi_0,vi_1,vi_2,vi_3, mesh_model); + this->addToMesh( + pt_m_0, pt_m_1, pt_m_2, pt_m_3, vi_0, vi_1, vi_2, vi_3, mesh_model); if (Mesh::IsManifold::value) // Only needed for the manifold mesh { - this->addToMesh (pt_m_0,pt_m_2,pt_m_4,pt_m_3, vi_0,vi_2,vi_4,vi_3, mesh_model); + this->addToMesh( + pt_m_0, pt_m_2, pt_m_4, pt_m_3, vi_0, vi_2, vi_4, vi_3, mesh_model); } } } @@ -181,87 +176,79 @@ pcl::ihs::Integration::reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_d //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data, - MeshPtr& mesh_model, - const Eigen::Matrix4f& T) const +pcl::ihs::Integration::merge(const CloudXYZRGBNormalConstPtr& cloud_data, + MeshPtr& mesh_model, + const Eigen::Matrix4f& T) const { - if (!cloud_data) - { + if (!cloud_data) { std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n"; return (false); } - if (!cloud_data->isOrganized ()) - { + if (!cloud_data->isOrganized()) { std::cerr << "ERROR in integration.cpp: Data cloud is not organized\n"; return (false); } - if (!mesh_model) - { + if (!mesh_model) { std::cerr << "ERROR in integration.cpp: Mesh pointer is invalid\n"; return (false); } - if (!mesh_model->sizeVertices ()) - { + if (!mesh_model->sizeVertices()) { std::cerr << "ERROR in integration.cpp: Model mesh is empty\n"; return (false); } - const int width = static_cast (cloud_data->width); - const int height = static_cast (cloud_data->height); + const int width = static_cast(cloud_data->width); + const int height = static_cast(cloud_data->height); // Nearest neighbor search - CloudXYZPtr xyz_model (new CloudXYZ ()); - xyz_model->reserve (mesh_model->sizeVertices ()); - for (std::size_t i=0; isizeVertices (); ++i) - { - const PointIHS& pt = mesh_model->getVertexDataCloud () [i]; - xyz_model->push_back (PointXYZ (pt.x, pt.y, pt.z)); + CloudXYZPtr xyz_model(new CloudXYZ()); + xyz_model->reserve(mesh_model->sizeVertices()); + for (std::size_t i = 0; i < mesh_model->sizeVertices(); ++i) { + const PointIHS& pt = mesh_model->getVertexDataCloud()[i]; + xyz_model->push_back(PointXYZ(pt.x, pt.y, pt.z)); } - kd_tree_->setInputCloud (xyz_model); - pcl::Indices index (1); - std::vector squared_distance (1); + kd_tree_->setInputCloud(xyz_model); + pcl::Indices index(1); + std::vector squared_distance(1); - mesh_model->reserveVertices (mesh_model->sizeVertices () + cloud_data->size ()); - mesh_model->reserveEdges (mesh_model->sizeEdges () + (width-1) * height + width * (height-1) + (width-1) * (height-1)); - mesh_model->reserveFaces (mesh_model->sizeFaces () + 2 * (width-1) * (height-1)); + mesh_model->reserveVertices(mesh_model->sizeVertices() + cloud_data->size()); + mesh_model->reserveEdges(mesh_model->sizeEdges() + (width - 1) * height + + width * (height - 1) + (width - 1) * (height - 1)); + mesh_model->reserveFaces(mesh_model->sizeFaces() + 2 * (width - 1) * (height - 1)); - // Data cloud in model coordinates (this does not change the connectivity information) and weights - CloudIHSPtr cloud_data_transformed (new CloudIHS ()); - cloud_data_transformed->resize (cloud_data->size ()); + // Data cloud in model coordinates (this does not change the connectivity information) + // and weights + CloudIHSPtr cloud_data_transformed(new CloudIHS()); + cloud_data_transformed->resize(cloud_data->size()); // Sensor position in model coordinates - const Eigen::Vector4f& sensor_eye = T * Eigen::Vector4f (0.f, 0.f, 0.f, 1.f); + const Eigen::Vector4f& sensor_eye = T * Eigen::Vector4f(0.f, 0.f, 0.f, 1.f); // Store which vertex is set at which position (initialized with invalid indices) - VertexIndices vertex_indices (cloud_data->size (), VertexIndex ()); + VertexIndices vertex_indices(cloud_data->size(), VertexIndex()); // Set the transformed points not reached by the main loop - for (int c=0; coperator [] (c); + for (int c = 0; c < width; ++c) { + const PointXYZRGBNormal& pt_d = cloud_data->operator[](c); const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1]) - if (!std::isnan (pt_d.x) && weight > min_weight_) - { - PointIHS& pt_d_t = cloud_data_transformed->operator [] (c); - pt_d_t = PointIHS (pt_d, weight); - pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap (); - pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap (); + if (!std::isnan(pt_d.x) && weight > min_weight_) { + PointIHS& pt_d_t = cloud_data_transformed->operator[](c); + pt_d_t = PointIHS(pt_d, weight); + pt_d_t.getVector4fMap() = T * pt_d_t.getVector4fMap(); + pt_d_t.getNormalVector4fMap() = T * pt_d_t.getNormalVector4fMap(); } } - for (int r=1; roperator [] (r*width + c); + for (int r = 1; r < height; ++r) { + for (int c = 0; c < 2; ++c) { + const PointXYZRGBNormal& pt_d = cloud_data->operator[](r * width + c); const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1]) - if (!std::isnan (pt_d.x) && weight > min_weight_) - { - PointIHS& pt_d_t = cloud_data_transformed->operator [] (r*width + c); - pt_d_t = PointIHS (pt_d, weight); - pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap (); - pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap (); + if (!std::isnan(pt_d.x) && weight > min_weight_) { + PointIHS& pt_d_t = cloud_data_transformed->operator[](r * width + c); + pt_d_t = PointIHS(pt_d, weight); + pt_d_t.getVector4fMap() = T * pt_d_t.getVector4fMap(); + pt_d_t.getNormalVector4fMap() = T * pt_d_t.getNormalVector4fMap(); } } } @@ -275,89 +262,92 @@ pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data, // * 3 - 0 // const int offset_1 = -width; const int offset_2 = -width - 1; - const int offset_3 = - 1; + const int offset_3 = -1; const int offset_4 = -width - 2; - const float dot_min = std::cos (max_angle_ * 17.45329252e-3); // deg to rad + const float dot_min = std::cos(max_angle_ * 17.45329252e-3); // deg to rad - for (int r=1; r= 0 && ind_0 < static_cast (cloud_data->size ())); - assert (ind_1 >= 0 && ind_1 < static_cast (cloud_data->size ())); - assert (ind_2 >= 0 && ind_2 < static_cast (cloud_data->size ())); - assert (ind_3 >= 0 && ind_3 < static_cast (cloud_data->size ())); - assert (ind_4 >= 0 && ind_4 < static_cast (cloud_data->size ())); + assert(ind_0 >= 0 && ind_0 < static_cast(cloud_data->size())); + assert(ind_1 >= 0 && ind_1 < static_cast(cloud_data->size())); + assert(ind_2 >= 0 && ind_2 < static_cast(cloud_data->size())); + assert(ind_3 >= 0 && ind_3 < static_cast(cloud_data->size())); + assert(ind_4 >= 0 && ind_4 < static_cast(cloud_data->size())); - const PointXYZRGBNormal& pt_d_0 = cloud_data->operator [] (ind_0); - PointIHS& pt_d_t_0 = cloud_data_transformed->operator [] (ind_0); - const PointIHS& pt_d_t_1 = cloud_data_transformed->operator [] (ind_1); - const PointIHS& pt_d_t_2 = cloud_data_transformed->operator [] (ind_2); - const PointIHS& pt_d_t_3 = cloud_data_transformed->operator [] (ind_3); - const PointIHS& pt_d_t_4 = cloud_data_transformed->operator [] (ind_4); + const PointXYZRGBNormal& pt_d_0 = cloud_data->operator[](ind_0); + PointIHS& pt_d_t_0 = cloud_data_transformed->operator[](ind_0); + const PointIHS& pt_d_t_1 = cloud_data_transformed->operator[](ind_1); + const PointIHS& pt_d_t_2 = cloud_data_transformed->operator[](ind_2); + const PointIHS& pt_d_t_3 = cloud_data_transformed->operator[](ind_3); + const PointIHS& pt_d_t_4 = cloud_data_transformed->operator[](ind_4); - VertexIndex& vi_0 = vertex_indices [ind_0]; + VertexIndex& vi_0 = vertex_indices[ind_0]; const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1]) - if (!std::isnan (pt_d_0.x) && weight > min_weight_) - { - pt_d_t_0 = PointIHS (pt_d_0, weight); - pt_d_t_0.getVector4fMap () = T * pt_d_t_0.getVector4fMap (); - pt_d_t_0.getNormalVector4fMap () = T * pt_d_t_0.getNormalVector4fMap (); + if (!std::isnan(pt_d_0.x) && weight > min_weight_) { + pt_d_t_0 = PointIHS(pt_d_0, weight); + pt_d_t_0.getVector4fMap() = T * pt_d_t_0.getVector4fMap(); + pt_d_t_0.getNormalVector4fMap() = T * pt_d_t_0.getNormalVector4fMap(); - pcl::PointXYZ tmp; tmp.getVector4fMap () = pt_d_t_0.getVector4fMap (); + pcl::PointXYZ tmp; + tmp.getVector4fMap() = pt_d_t_0.getVector4fMap(); // NN search - if (!kd_tree_->nearestKSearch (tmp, 1, index, squared_distance)) - { + if (!kd_tree_->nearestKSearch(tmp, 1, index, squared_distance)) { std::cerr << "ERROR in integration.cpp: nearestKSearch failed!\n"; return (false); } // Average out corresponding points - if (squared_distance [0] <= max_squared_distance_) - { - PointIHS& pt_m = mesh_model->getVertexDataCloud () [index [0]]; // Non-const reference! - - if (pt_m.getNormalVector4fMap ().dot (pt_d_t_0.getNormalVector4fMap ()) >= dot_min) - { - vi_0 = VertexIndex (index [0]); - - const float W = pt_m.weight; // Old accumulated weight - const float w = pt_d_t_0.weight; // Weight of new point - const float WW = pt_m.weight = W + w; // New accumulated weight - - const float r_m = static_cast (pt_m.r); - const float g_m = static_cast (pt_m.g); - const float b_m = static_cast (pt_m.b); - - const float r_d = static_cast (pt_d_t_0.r); - const float g_d = static_cast (pt_d_t_0.g); - const float b_d = static_cast (pt_d_t_0.b); - - pt_m.getVector4fMap () = ( W*pt_m.getVector4fMap () + w*pt_d_t_0.getVector4fMap ()) / WW; - pt_m.getNormalVector4fMap () = ((W*pt_m.getNormalVector4fMap () + w*pt_d_t_0.getNormalVector4fMap ()) / WW).normalized (); - pt_m.r = this->trimRGB ((W*r_m + w*r_d) / WW); - pt_m.g = this->trimRGB ((W*g_m + w*g_d) / WW); - pt_m.b = this->trimRGB ((W*b_m + w*b_d) / WW); + if (squared_distance[0] <= max_squared_distance_) { + PointIHS& pt_m = + mesh_model->getVertexDataCloud()[index[0]]; // Non-const reference! + + if (pt_m.getNormalVector4fMap().dot(pt_d_t_0.getNormalVector4fMap()) >= + dot_min) { + vi_0 = VertexIndex(index[0]); + + const float W = pt_m.weight; // Old accumulated weight + const float w = pt_d_t_0.weight; // Weight of new point + const float WW = pt_m.weight = W + w; // New accumulated weight + + const float r_m = static_cast(pt_m.r); + const float g_m = static_cast(pt_m.g); + const float b_m = static_cast(pt_m.b); + + const float r_d = static_cast(pt_d_t_0.r); + const float g_d = static_cast(pt_d_t_0.g); + const float b_d = static_cast(pt_d_t_0.b); + + pt_m.getVector4fMap() = + (W * pt_m.getVector4fMap() + w * pt_d_t_0.getVector4fMap()) / WW; + pt_m.getNormalVector4fMap() = ((W * pt_m.getNormalVector4fMap() + + w * pt_d_t_0.getNormalVector4fMap()) / + WW) + .normalized(); + pt_m.r = this->trimRGB((W * r_m + w * r_d) / WW); + pt_m.g = this->trimRGB((W * g_m + w * g_d) / WW); + pt_m.b = this->trimRGB((W * b_m + w * b_d) / WW); // Point has been observed again -> give it some extra time to live pt_m.age = 0; // Add a direction - pcl::ihs::addDirection (pt_m.getNormalVector4fMap (), sensor_eye-pt_m.getVector4fMap (), pt_m.directions); + pcl::ihs::addDirection(pt_m.getNormalVector4fMap(), + sensor_eye - pt_m.getVector4fMap(), + pt_m.directions); } // dot normals - } // squared distance - } // !isnan && min weight + } // squared distance + } // !isnan && min weight // Connect // 4 2 - 1 // @@ -368,15 +358,17 @@ pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data, // \ \ // // * 3 - 0 // - VertexIndex& vi_1 = vertex_indices [ind_1]; - VertexIndex& vi_2 = vertex_indices [ind_2]; - VertexIndex& vi_3 = vertex_indices [ind_3]; - VertexIndex& vi_4 = vertex_indices [ind_4]; + VertexIndex& vi_1 = vertex_indices[ind_1]; + VertexIndex& vi_2 = vertex_indices[ind_2]; + VertexIndex& vi_3 = vertex_indices[ind_3]; + VertexIndex& vi_4 = vertex_indices[ind_4]; - this->addToMesh (pt_d_t_0,pt_d_t_1,pt_d_t_2,pt_d_t_3, vi_0,vi_1,vi_2,vi_3, mesh_model); + this->addToMesh( + pt_d_t_0, pt_d_t_1, pt_d_t_2, pt_d_t_3, vi_0, vi_1, vi_2, vi_3, mesh_model); if (Mesh::IsManifold::value) // Only needed for the manifold mesh { - this->addToMesh (pt_d_t_0,pt_d_t_2,pt_d_t_4,pt_d_t_3, vi_0,vi_2,vi_4,vi_3, mesh_model); + this->addToMesh( + pt_d_t_0, pt_d_t_2, pt_d_t_4, pt_d_t_3, vi_0, vi_2, vi_4, vi_3, mesh_model); } } } @@ -387,67 +379,62 @@ pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data, //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const +pcl::ihs::Integration::age(const MeshPtr& mesh, const bool cleanup) const { - for (std::size_t i=0; isizeVertices (); ++i) - { - PointIHS& pt = mesh->getVertexDataCloud () [i]; - if (pt.age < max_age_) - { + for (std::size_t i = 0; i < mesh->sizeVertices(); ++i) { + PointIHS& pt = mesh->getVertexDataCloud()[i]; + if (pt.age < max_age_) { // Point survives ++(pt.age); } else if (pt.age == max_age_) // Judgement Day { - if (pcl::ihs::countDirections (pt.directions) < min_directions_) - { + if (pcl::ihs::countDirections(pt.directions) < min_directions_) { // Point dies (no need to transform it) - mesh->deleteVertex (VertexIndex (i)); + mesh->deleteVertex(VertexIndex(i)); } - else - { + else { // Point becomes immortal - pt.age = std::numeric_limits ::max (); + pt.age = std::numeric_limits::max(); } } } - if (cleanup) - { - mesh->cleanUp (); + if (cleanup) { + mesh->cleanUp(); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::removeUnfitVertices (const MeshPtr& mesh, const bool cleanup) const +pcl::ihs::Integration::removeUnfitVertices(const MeshPtr& mesh, + const bool cleanup) const { - for (std::size_t i=0; isizeVertices (); ++i) - { - if (pcl::ihs::countDirections (mesh->getVertexDataCloud () [i].directions) < min_directions_) - { + for (std::size_t i = 0; i < mesh->sizeVertices(); ++i) { + if (pcl::ihs::countDirections(mesh->getVertexDataCloud()[i].directions) < + min_directions_) { // Point dies (no need to transform it) - mesh->deleteVertex (VertexIndex (i)); + mesh->deleteVertex(VertexIndex(i)); } } - if (cleanup) - { - mesh->cleanUp (); + if (cleanup) { + mesh->cleanUp(); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::setMaxSquaredDistance (const float squared_distance) +pcl::ihs::Integration::setMaxSquaredDistance(const float squared_distance) { - if (squared_distance > 0) max_squared_distance_ = squared_distance; + if (squared_distance > 0) + max_squared_distance_ = squared_distance; } float -pcl::ihs::Integration::getMaxSquaredDistance () const +pcl::ihs::Integration::getMaxSquaredDistance() const { return (max_squared_distance_); } @@ -455,13 +442,13 @@ pcl::ihs::Integration::getMaxSquaredDistance () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::setMaxAngle (const float angle) +pcl::ihs::Integration::setMaxAngle(const float angle) { - max_angle_ = pcl::ihs::clamp (angle, 0.f, 180.f); + max_angle_ = pcl::ihs::clamp(angle, 0.f, 180.f); } float -pcl::ihs::Integration::getMaxAngle () const +pcl::ihs::Integration::getMaxAngle() const { return (max_angle_); } @@ -469,13 +456,13 @@ pcl::ihs::Integration::getMaxAngle () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::setMaxAge (const unsigned int age) +pcl::ihs::Integration::setMaxAge(const unsigned int age) { max_age_ = age < 1 ? 1 : age; } unsigned int -pcl::ihs::Integration::getMaxAge () const +pcl::ihs::Integration::getMaxAge() const { return (max_age_); } @@ -483,13 +470,13 @@ pcl::ihs::Integration::getMaxAge () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::setMinDirections (const unsigned int directions) +pcl::ihs::Integration::setMinDirections(const unsigned int directions) { min_directions_ = directions < 1 ? 1 : directions; } unsigned int -pcl::ihs::Integration::getMinDirections () const +pcl::ihs::Integration::getMinDirections() const { return (min_directions_); } @@ -497,106 +484,133 @@ pcl::ihs::Integration::getMinDirections () const //////////////////////////////////////////////////////////////////////////////// std::uint8_t -pcl::ihs::Integration::trimRGB (const float val) const +pcl::ihs::Integration::trimRGB(const float val) const { - return (static_cast (val > 255.f ? 255 : val)); + return (static_cast(val > 255.f ? 255 : val)); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::addToMesh (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2, - const PointIHS& pt_3, - VertexIndex& vi_0, - VertexIndex& vi_1, - VertexIndex& vi_2, - VertexIndex& vi_3, - const MeshPtr& mesh) const +pcl::ihs::Integration::addToMesh(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2, + const PointIHS& pt_3, + VertexIndex& vi_0, + VertexIndex& vi_1, + VertexIndex& vi_2, + VertexIndex& vi_3, + const MeshPtr& mesh) const { // Treated bitwise // 2 - 1 // | | // 3 - 0 - const unsigned char is_finite = static_cast ( - (1 * !std::isnan (pt_0.x)) | - (2 * !std::isnan (pt_1.x)) | - (4 * !std::isnan (pt_2.x)) | - (8 * !std::isnan (pt_3.x))); - - switch (is_finite) + const unsigned char is_finite = + static_cast((1 * !std::isnan(pt_0.x)) | (2 * !std::isnan(pt_1.x)) | + (4 * !std::isnan(pt_2.x)) | (8 * !std::isnan(pt_3.x))); + + switch (is_finite) { + case 7: + this->addToMesh(pt_0, pt_1, pt_2, vi_0, vi_1, vi_2, mesh); + break; // 0-1-2 + case 11: + this->addToMesh(pt_0, pt_1, pt_3, vi_0, vi_1, vi_3, mesh); + break; // 0-1-3 + case 13: + this->addToMesh(pt_0, pt_2, pt_3, vi_0, vi_2, vi_3, mesh); + break; // 0-2-3 + case 14: + this->addToMesh(pt_1, pt_2, pt_3, vi_1, vi_2, vi_3, mesh); + break; // 1-2-3 + case 15: // 0-1-2-3 { - case 7: this->addToMesh (pt_0, pt_1, pt_2, vi_0, vi_1, vi_2, mesh); break; // 0-1-2 - case 11: this->addToMesh (pt_0, pt_1, pt_3, vi_0, vi_1, vi_3, mesh); break; // 0-1-3 - case 13: this->addToMesh (pt_0, pt_2, pt_3, vi_0, vi_2, vi_3, mesh); break; // 0-2-3 - case 14: this->addToMesh (pt_1, pt_2, pt_3, vi_1, vi_2, vi_3, mesh); break; // 1-2-3 - case 15: // 0-1-2-3 - { - if (!distanceThreshold (pt_0, pt_1, pt_2, pt_3)) break; - if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0); - if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1); - if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2); - if (!vi_3.isValid ()) vi_3 = mesh->addVertex (pt_3); - if (vi_0==vi_1 || vi_0==vi_2 || vi_0==vi_3 || vi_1==vi_2 || vi_1==vi_3 || vi_2==vi_3) - { - return; - } - mesh->addTrianglePair (vi_0, vi_1, vi_2, vi_3); - + if (!distanceThreshold(pt_0, pt_1, pt_2, pt_3)) break; + if (!vi_0.isValid()) + vi_0 = mesh->addVertex(pt_0); + if (!vi_1.isValid()) + vi_1 = mesh->addVertex(pt_1); + if (!vi_2.isValid()) + vi_2 = mesh->addVertex(pt_2); + if (!vi_3.isValid()) + vi_3 = mesh->addVertex(pt_3); + if (vi_0 == vi_1 || vi_0 == vi_2 || vi_0 == vi_3 || vi_1 == vi_2 || vi_1 == vi_3 || + vi_2 == vi_3) { + return; } + mesh->addTrianglePair(vi_0, vi_1, vi_2, vi_3); + + break; + } } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::Integration::addToMesh (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2, - VertexIndex& vi_0, - VertexIndex& vi_1, - VertexIndex& vi_2, - const MeshPtr& mesh) const +pcl::ihs::Integration::addToMesh(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2, + VertexIndex& vi_0, + VertexIndex& vi_1, + VertexIndex& vi_2, + const MeshPtr& mesh) const { - if (!distanceThreshold (pt_0, pt_1, pt_2)) return; + if (!distanceThreshold(pt_0, pt_1, pt_2)) + return; - if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0); - if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1); - if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2); - if (vi_0==vi_1 || vi_0==vi_2 || vi_1==vi_2) - { + if (!vi_0.isValid()) + vi_0 = mesh->addVertex(pt_0); + if (!vi_1.isValid()) + vi_1 = mesh->addVertex(pt_1); + if (!vi_2.isValid()) + vi_2 = mesh->addVertex(pt_2); + if (vi_0 == vi_1 || vi_0 == vi_2 || vi_1 == vi_2) { return; } - mesh->addFace (vi_0, vi_1, vi_2); + mesh->addFace(vi_0, vi_1, vi_2); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::Integration::distanceThreshold (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2) const +pcl::ihs::Integration::distanceThreshold(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2) const { - if ((pt_0.getVector3fMap () - pt_1.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); - if ((pt_1.getVector3fMap () - pt_2.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); - if ((pt_2.getVector3fMap () - pt_0.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); + if ((pt_0.getVector3fMap() - pt_1.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); + if ((pt_1.getVector3fMap() - pt_2.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); + if ((pt_2.getVector3fMap() - pt_0.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); return (true); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::Integration::distanceThreshold (const PointIHS& pt_0, - const PointIHS& pt_1, - const PointIHS& pt_2, - const PointIHS& pt_3) const +pcl::ihs::Integration::distanceThreshold(const PointIHS& pt_0, + const PointIHS& pt_1, + const PointIHS& pt_2, + const PointIHS& pt_3) const { - if ((pt_0.getVector3fMap () - pt_1.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); - if ((pt_1.getVector3fMap () - pt_2.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); - if ((pt_2.getVector3fMap () - pt_3.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); - if ((pt_3.getVector3fMap () - pt_0.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false); + if ((pt_0.getVector3fMap() - pt_1.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); + if ((pt_1.getVector3fMap() - pt_2.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); + if ((pt_2.getVector3fMap() - pt_3.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); + if ((pt_3.getVector3fMap() - pt_0.getVector3fMap()).squaredNorm() > + max_squared_distance_) + return (false); return (true); } diff --git a/apps/in_hand_scanner/src/main.cpp b/apps/in_hand_scanner/src/main.cpp index d404dc0c..05c4e03b 100644 --- a/apps/in_hand_scanner/src/main.cpp +++ b/apps/in_hand_scanner/src/main.cpp @@ -38,15 +38,15 @@ * */ -#include - #include +#include + int -main (int argc, char** argv) +main(int argc, char** argv) { - QApplication app (argc, argv); + QApplication app(argc, argv); pcl::ihs::MainWindow mw; - mw.show (); - return (QApplication::exec ()); + mw.show(); + return (QApplication::exec()); } diff --git a/apps/in_hand_scanner/src/main_offline_integration.cpp b/apps/in_hand_scanner/src/main_offline_integration.cpp index a8edc1bf..5d2c36c6 100644 --- a/apps/in_hand_scanner/src/main_offline_integration.cpp +++ b/apps/in_hand_scanner/src/main_offline_integration.cpp @@ -38,17 +38,17 @@ * */ +#include + #include #include -#include - int -main (int argc, char** argv) +main(int argc, char** argv) { - QApplication app (argc, argv); + QApplication app(argc, argv); pcl::ihs::OfflineIntegration oi; - QTimer::singleShot(0, &oi, SLOT (start ())); - oi.show (); - return (QApplication::exec ()); + QTimer::singleShot(0, &oi, SLOT(start())); + oi.show(); + return (QApplication::exec()); } diff --git a/apps/in_hand_scanner/src/main_window.cpp b/apps/in_hand_scanner/src/main_window.cpp index 8f21294c..f3260130 100644 --- a/apps/in_hand_scanner/src/main_window.cpp +++ b/apps/in_hand_scanner/src/main_window.cpp @@ -38,10 +38,12 @@ * */ +#include +#include +#include +#include +#include #include -#include "ui_main_window.h" - -#include #include #include @@ -50,339 +52,387 @@ #include #include -#include -#include -#include -#include -#include +#include "ui_main_window.h" + +#include //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::MainWindow::MainWindow (QWidget* parent) - : QMainWindow (parent), - ui_ (new Ui::MainWindow ()), - help_window_ (new HelpWindow (this)), - ihs_ (new InHandScanner ()) +pcl::ihs::MainWindow::MainWindow(QWidget* parent) +: QMainWindow(parent) +, ui_(new Ui::MainWindow()) +, help_window_(new HelpWindow(this)) +, ihs_(new InHandScanner()) { - ui_->setupUi (this); + ui_->setupUi(this); - QWidget* spacer = new QWidget (); - spacer->setSizePolicy (QSizePolicy::Expanding, QSizePolicy::Expanding); - ui_->toolBar->insertWidget (ui_->actionHelp, spacer); + QWidget* spacer = new QWidget(); + spacer->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + ui_->toolBar->insertWidget(ui_->actionHelp, spacer); - const double max = std::numeric_limits ::max (); + const double max = std::numeric_limits::max(); // In hand scanner - QHBoxLayout* layout = new QHBoxLayout (ui_->placeholder_in_hand_scanner); - layout->addWidget (ihs_); + QHBoxLayout* layout = new QHBoxLayout(ui_->placeholder_in_hand_scanner); + layout->addWidget(ihs_); // ui_->centralWidget->setLayout (layout); - QTimer::singleShot (0, ihs_, SLOT (startGrabber ())); + QTimer::singleShot(0, ihs_, SLOT(startGrabber())); - connect (ui_->toolButton_1, SIGNAL (clicked ()), ihs_, SLOT (showUnprocessedData ())); - connect (ui_->toolButton_2, SIGNAL (clicked ()), ihs_, SLOT (showProcessedData ())); - connect (ui_->toolButton_3, SIGNAL (clicked ()), ihs_, SLOT (registerContinuously ())); - connect (ui_->toolButton_4, SIGNAL (clicked ()), ihs_, SLOT (registerOnce ())); - connect (ui_->toolButton_5, SIGNAL (clicked ()), ihs_, SLOT (showModel ())); - connect (ui_->toolButton_6, SIGNAL (clicked ()), ihs_, SLOT (removeUnfitVertices ())); - connect (ui_->toolButton_0, SIGNAL (clicked ()), ihs_, SLOT (reset ())); + connect(ui_->toolButton_1, SIGNAL(clicked()), ihs_, SLOT(showUnprocessedData())); + connect(ui_->toolButton_2, SIGNAL(clicked()), ihs_, SLOT(showProcessedData())); + connect(ui_->toolButton_3, SIGNAL(clicked()), ihs_, SLOT(registerContinuously())); + connect(ui_->toolButton_4, SIGNAL(clicked()), ihs_, SLOT(registerOnce())); + connect(ui_->toolButton_5, SIGNAL(clicked()), ihs_, SLOT(showModel())); + connect(ui_->toolButton_6, SIGNAL(clicked()), ihs_, SLOT(removeUnfitVertices())); + connect(ui_->toolButton_0, SIGNAL(clicked()), ihs_, SLOT(reset())); - connect (ui_->actionReset_camera, SIGNAL (triggered ()), ihs_, SLOT (resetCamera ())); - connect (ui_->actionToggle_coloring, SIGNAL (triggered ()), ihs_, SLOT (toggleColoring ())); - connect (ui_->actionMesh_representation, SIGNAL (triggered ()), ihs_, SLOT (toggleMeshRepresentation ())); + connect(ui_->actionReset_camera, SIGNAL(triggered()), ihs_, SLOT(resetCamera())); + connect( + ui_->actionToggle_coloring, SIGNAL(triggered()), ihs_, SLOT(toggleColoring())); + connect(ui_->actionMesh_representation, + SIGNAL(triggered()), + ihs_, + SLOT(toggleMeshRepresentation())); - connect (ui_->actionSaveAs, SIGNAL (triggered ()), this, SLOT (saveAs ())); + connect(ui_->actionSaveAs, SIGNAL(triggered()), this, SLOT(saveAs())); - connect (ihs_, SIGNAL (runningModeChanged (RunningMode)), this, SLOT (runningModeChanged (RunningMode))); + connect(ihs_, + SIGNAL(runningModeChanged(RunningMode)), + this, + SLOT(runningModeChanged(RunningMode))); // Input data processing - const pcl::ihs::InputDataProcessing& idp = ihs_->getInputDataProcessing (); - - ui_->spinBox_x_min->setValue (static_cast (idp.getXMin ())); - ui_->spinBox_x_max->setValue (static_cast (idp.getXMax ())); - ui_->spinBox_y_min->setValue (static_cast (idp.getYMin ())); - ui_->spinBox_y_max->setValue (static_cast (idp.getYMax ())); - ui_->spinBox_z_min->setValue (static_cast (idp.getZMin ())); - ui_->spinBox_z_max->setValue (static_cast (idp.getZMax ())); - - ui_->spinBox_h_min->setValue (static_cast (idp.getHMin ())); - ui_->spinBox_h_max->setValue (static_cast (idp.getHMax ())); - ui_->spinBox_s_min->setValue (static_cast (idp.getSMin () * 100.f)); - ui_->spinBox_s_max->setValue (static_cast (idp.getSMax () * 100.f)); - ui_->spinBox_v_min->setValue (static_cast (idp.getVMin () * 100.f)); - ui_->spinBox_v_max->setValue (static_cast (idp.getVMax () * 100.f)); - - ui_->checkBox_color_segmentation_inverted->setChecked (idp.getColorSegmentationInverted ()); - ui_->checkBox_color_segmentation_enabled->setChecked (idp.getColorSegmentationEnabled ()); - - ui_->spinBox_xyz_erode_size->setValue (idp.getXYZErodeSize ()); - ui_->spinBox_hsv_dilate_size->setValue (idp.getHSVDilateSize ()); + const pcl::ihs::InputDataProcessing& idp = ihs_->getInputDataProcessing(); + + ui_->spinBox_x_min->setValue(static_cast(idp.getXMin())); + ui_->spinBox_x_max->setValue(static_cast(idp.getXMax())); + ui_->spinBox_y_min->setValue(static_cast(idp.getYMin())); + ui_->spinBox_y_max->setValue(static_cast(idp.getYMax())); + ui_->spinBox_z_min->setValue(static_cast(idp.getZMin())); + ui_->spinBox_z_max->setValue(static_cast(idp.getZMax())); + + ui_->spinBox_h_min->setValue(static_cast(idp.getHMin())); + ui_->spinBox_h_max->setValue(static_cast(idp.getHMax())); + ui_->spinBox_s_min->setValue(static_cast(idp.getSMin() * 100.f)); + ui_->spinBox_s_max->setValue(static_cast(idp.getSMax() * 100.f)); + ui_->spinBox_v_min->setValue(static_cast(idp.getVMin() * 100.f)); + ui_->spinBox_v_max->setValue(static_cast(idp.getVMax() * 100.f)); + + ui_->checkBox_color_segmentation_inverted->setChecked( + idp.getColorSegmentationInverted()); + ui_->checkBox_color_segmentation_enabled->setChecked( + idp.getColorSegmentationEnabled()); + + ui_->spinBox_xyz_erode_size->setValue(idp.getXYZErodeSize()); + ui_->spinBox_hsv_dilate_size->setValue(idp.getHSVDilateSize()); // Registration - ui_->lineEdit_epsilon->setValidator (new QDoubleValidator (0., max, 2)); - ui_->lineEdit_max_fitness->setValidator (new QDoubleValidator (0., max, 2)); + ui_->lineEdit_epsilon->setValidator(new QDoubleValidator(0., max, 2)); + ui_->lineEdit_max_fitness->setValidator(new QDoubleValidator(0., max, 2)); - ui_->lineEdit_epsilon->setText (QString ().setNum (ihs_->getICP ().getEpsilon ())); - ui_->spinBox_max_iterations->setValue (static_cast (ihs_->getICP ().getMaxIterations ())); - ui_->spinBox_min_overlap->setValue (static_cast (100.f * ihs_->getICP ().getMinOverlap ())); - ui_->lineEdit_max_fitness->setText (QString ().setNum (ihs_->getICP ().getMaxFitness ())); + ui_->lineEdit_epsilon->setText(QString().setNum(ihs_->getICP().getEpsilon())); + ui_->spinBox_max_iterations->setValue( + static_cast(ihs_->getICP().getMaxIterations())); + ui_->spinBox_min_overlap->setValue( + static_cast(100.f * ihs_->getICP().getMinOverlap())); + ui_->lineEdit_max_fitness->setText(QString().setNum(ihs_->getICP().getMaxFitness())); - ui_->doubleSpinBox_correspondence_rejection_factor->setValue (ihs_->getICP ().getCorrespondenceRejectionFactor ()); - ui_->spinBox_correspondence_rejection_max_angle->setValue (static_cast (ihs_->getICP ().getMaxAngle ())); + ui_->doubleSpinBox_correspondence_rejection_factor->setValue( + ihs_->getICP().getCorrespondenceRejectionFactor()); + ui_->spinBox_correspondence_rejection_max_angle->setValue( + static_cast(ihs_->getICP().getMaxAngle())); // Integration - ui_->lineEdit_max_squared_distance->setValidator (new QDoubleValidator (0., max, 2)); + ui_->lineEdit_max_squared_distance->setValidator(new QDoubleValidator(0., max, 2)); - ui_->lineEdit_max_squared_distance->setText (QString ().setNum (ihs_->getIntegration ().getMaxSquaredDistance ())); - ui_->spinBox_averaging_max_angle->setValue (static_cast (ihs_->getIntegration ().getMaxAngle ())); - ui_->spinBox_max_age->setValue (static_cast (ihs_->getIntegration ().getMaxAge ())); - ui_->spinBox_min_directions->setValue (static_cast (ihs_->getIntegration ().getMinDirections ())); + ui_->lineEdit_max_squared_distance->setText( + QString().setNum(ihs_->getIntegration().getMaxSquaredDistance())); + ui_->spinBox_averaging_max_angle->setValue( + static_cast(ihs_->getIntegration().getMaxAngle())); + ui_->spinBox_max_age->setValue(static_cast(ihs_->getIntegration().getMaxAge())); + ui_->spinBox_min_directions->setValue( + static_cast(ihs_->getIntegration().getMinDirections())); // Help - connect (ui_->actionHelp, SIGNAL (triggered ()), this, SLOT (showHelp ())); + connect(ui_->actionHelp, SIGNAL(triggered()), this, SLOT(showHelp())); } //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::MainWindow::~MainWindow () -{ - delete ui_; -} +pcl::ihs::MainWindow::~MainWindow() { delete ui_; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::showHelp () +pcl::ihs::MainWindow::showHelp() { - help_window_->show (); + help_window_->show(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::saveAs () +pcl::ihs::MainWindow::saveAs() { - QString filename = QFileDialog::getSaveFileName (this, "Save the model mesh.", "", "Polygon File Format (*.ply);;VTK File Format (*.vtk)"); + QString filename = QFileDialog::getSaveFileName( + this, + "Save the model mesh.", + "", + "Polygon File Format (*.ply);;VTK File Format (*.vtk)"); - if (filename.isEmpty ()) return; + if (filename.isEmpty()) + return; - if (filename.endsWith ("ply", Qt::CaseInsensitive)) - ihs_->saveAs (filename.toStdString (), pcl::ihs::InHandScanner::FT_PLY); - else if (filename.endsWith ("vtk", Qt::CaseInsensitive)) - ihs_->saveAs (filename.toStdString (), pcl::ihs::InHandScanner::FT_VTK); + if (filename.endsWith("ply", Qt::CaseInsensitive)) + ihs_->saveAs(filename.toStdString(), pcl::ihs::InHandScanner::FT_PLY); + else if (filename.endsWith("vtk", Qt::CaseInsensitive)) + ihs_->saveAs(filename.toStdString(), pcl::ihs::InHandScanner::FT_VTK); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::runningModeChanged (const RunningMode mode) -{ - switch (mode) - { - case InHandScanner::RM_UNPROCESSED: ui_->toolButton_1->setChecked (true); break; - case InHandScanner::RM_PROCESSED: ui_->toolButton_2->setChecked (true); break; - case InHandScanner::RM_REGISTRATION_CONT: ui_->toolButton_3->setChecked (true); break; - case InHandScanner::RM_REGISTRATION_SINGLE: break; - case InHandScanner::RM_SHOW_MODEL: ui_->toolButton_5->setChecked (true); break; +pcl::ihs::MainWindow::runningModeChanged(const RunningMode mode) +{ + switch (mode) { + case InHandScanner::RM_UNPROCESSED: + ui_->toolButton_1->setChecked(true); + break; + case InHandScanner::RM_PROCESSED: + ui_->toolButton_2->setChecked(true); + break; + case InHandScanner::RM_REGISTRATION_CONT: + ui_->toolButton_3->setChecked(true); + break; + case InHandScanner::RM_REGISTRATION_SINGLE: + break; + case InHandScanner::RM_SHOW_MODEL: + ui_->toolButton_5->setChecked(true); + break; } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::keyPressEvent (QKeyEvent* event) +pcl::ihs::MainWindow::keyPressEvent(QKeyEvent* event) { - ihs_->keyPressEvent (event); + ihs_->keyPressEvent(event); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::setXMin (const int x_min) +pcl::ihs::MainWindow::setXMin(const int x_min) { - ihs_->getInputDataProcessing ().setXMin (static_cast (x_min)); - ui_->spinBox_x_min->setValue (static_cast (ihs_->getInputDataProcessing ().getXMin ())); + ihs_->getInputDataProcessing().setXMin(static_cast(x_min)); + ui_->spinBox_x_min->setValue( + static_cast(ihs_->getInputDataProcessing().getXMin())); } void -pcl::ihs::MainWindow::setXMax (const int x_max) +pcl::ihs::MainWindow::setXMax(const int x_max) { - ihs_->getInputDataProcessing ().setXMax (static_cast (x_max)); - ui_->spinBox_x_max->setValue (static_cast (ihs_->getInputDataProcessing ().getXMax ())); + ihs_->getInputDataProcessing().setXMax(static_cast(x_max)); + ui_->spinBox_x_max->setValue( + static_cast(ihs_->getInputDataProcessing().getXMax())); } void -pcl::ihs::MainWindow::setYMin (const int y_min) +pcl::ihs::MainWindow::setYMin(const int y_min) { - ihs_->getInputDataProcessing ().setYMin (static_cast (y_min)); - ui_->spinBox_y_min->setValue (static_cast (ihs_->getInputDataProcessing ().getYMin ())); + ihs_->getInputDataProcessing().setYMin(static_cast(y_min)); + ui_->spinBox_y_min->setValue( + static_cast(ihs_->getInputDataProcessing().getYMin())); } void -pcl::ihs::MainWindow::setYMax (const int y_max) +pcl::ihs::MainWindow::setYMax(const int y_max) { - ihs_->getInputDataProcessing ().setYMax (static_cast (y_max)); - ui_->spinBox_y_max->setValue (static_cast (ihs_->getInputDataProcessing ().getYMax ())); + ihs_->getInputDataProcessing().setYMax(static_cast(y_max)); + ui_->spinBox_y_max->setValue( + static_cast(ihs_->getInputDataProcessing().getYMax())); } void -pcl::ihs::MainWindow::setZMin (const int z_min) +pcl::ihs::MainWindow::setZMin(const int z_min) { - ihs_->getInputDataProcessing ().setZMin (static_cast (z_min)); - ui_->spinBox_z_min->setValue (static_cast (ihs_->getInputDataProcessing ().getZMin ())); + ihs_->getInputDataProcessing().setZMin(static_cast(z_min)); + ui_->spinBox_z_min->setValue( + static_cast(ihs_->getInputDataProcessing().getZMin())); } void -pcl::ihs::MainWindow::setZMax (const int z_max) +pcl::ihs::MainWindow::setZMax(const int z_max) { - ihs_->getInputDataProcessing ().setZMax (static_cast (z_max)); - ui_->spinBox_z_max->setValue (static_cast (ihs_->getInputDataProcessing ().getZMax ())); + ihs_->getInputDataProcessing().setZMax(static_cast(z_max)); + ui_->spinBox_z_max->setValue( + static_cast(ihs_->getInputDataProcessing().getZMax())); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::setHMin (const int h_min) +pcl::ihs::MainWindow::setHMin(const int h_min) { - ihs_->getInputDataProcessing ().setHMin (static_cast (h_min)); - ui_->spinBox_h_min->setValue (static_cast (ihs_->getInputDataProcessing ().getHMin ())); + ihs_->getInputDataProcessing().setHMin(static_cast(h_min)); + ui_->spinBox_h_min->setValue( + static_cast(ihs_->getInputDataProcessing().getHMin())); } void -pcl::ihs::MainWindow::setHMax (const int h_max) +pcl::ihs::MainWindow::setHMax(const int h_max) { - ihs_->getInputDataProcessing ().setHMax (static_cast (h_max)); - ui_->spinBox_h_max->setValue (static_cast (ihs_->getInputDataProcessing ().getHMax ())); + ihs_->getInputDataProcessing().setHMax(static_cast(h_max)); + ui_->spinBox_h_max->setValue( + static_cast(ihs_->getInputDataProcessing().getHMax())); } void -pcl::ihs::MainWindow::setSMin (const int s_min) +pcl::ihs::MainWindow::setSMin(const int s_min) { - ihs_->getInputDataProcessing ().setSMin (.01f * static_cast (s_min)); - ui_->spinBox_s_min->setValue (static_cast (100.f * ihs_->getInputDataProcessing ().getSMin () + 0.5f)); + ihs_->getInputDataProcessing().setSMin(.01f * static_cast(s_min)); + ui_->spinBox_s_min->setValue( + static_cast(100.f * ihs_->getInputDataProcessing().getSMin() + 0.5f)); } void -pcl::ihs::MainWindow::setSMax (const int s_max) +pcl::ihs::MainWindow::setSMax(const int s_max) { - ihs_->getInputDataProcessing ().setSMax (.01f * static_cast (s_max)); - ui_->spinBox_s_max->setValue (static_cast (100.f * ihs_->getInputDataProcessing ().getSMax () + 0.5f)); + ihs_->getInputDataProcessing().setSMax(.01f * static_cast(s_max)); + ui_->spinBox_s_max->setValue( + static_cast(100.f * ihs_->getInputDataProcessing().getSMax() + 0.5f)); } void -pcl::ihs::MainWindow::setVMin (const int v_min) +pcl::ihs::MainWindow::setVMin(const int v_min) { - ihs_->getInputDataProcessing ().setVMin (.01f * static_cast (v_min)); - ui_->spinBox_v_min->setValue (static_cast (100.f * ihs_->getInputDataProcessing ().getVMin () + 0.5f)); + ihs_->getInputDataProcessing().setVMin(.01f * static_cast(v_min)); + ui_->spinBox_v_min->setValue( + static_cast(100.f * ihs_->getInputDataProcessing().getVMin() + 0.5f)); } void -pcl::ihs::MainWindow::setVMax (const int v_max) +pcl::ihs::MainWindow::setVMax(const int v_max) { - ihs_->getInputDataProcessing ().setVMax (.01f * static_cast (v_max)); - ui_->spinBox_v_max->setValue (static_cast (100.f * ihs_->getInputDataProcessing ().getVMax () + 0.5f)); + ihs_->getInputDataProcessing().setVMax(.01f * static_cast(v_max)); + ui_->spinBox_v_max->setValue( + static_cast(100.f * ihs_->getInputDataProcessing().getVMax() + 0.5f)); } void -pcl::ihs::MainWindow::setColorSegmentationInverted (const bool is_inverted) +pcl::ihs::MainWindow::setColorSegmentationInverted(const bool is_inverted) { - ihs_->getInputDataProcessing ().setColorSegmentationInverted (is_inverted); - ui_->checkBox_color_segmentation_inverted->setChecked (ihs_->getInputDataProcessing ().getColorSegmentationInverted ()); + ihs_->getInputDataProcessing().setColorSegmentationInverted(is_inverted); + ui_->checkBox_color_segmentation_inverted->setChecked( + ihs_->getInputDataProcessing().getColorSegmentationInverted()); } void -pcl::ihs::MainWindow::setColorSegmentationEnabled (const bool is_enabled) +pcl::ihs::MainWindow::setColorSegmentationEnabled(const bool is_enabled) { - ihs_->getInputDataProcessing ().setColorSegmentationEnabled (is_enabled); - ui_->checkBox_color_segmentation_enabled->setChecked (ihs_->getInputDataProcessing ().getColorSegmentationEnabled ()); + ihs_->getInputDataProcessing().setColorSegmentationEnabled(is_enabled); + ui_->checkBox_color_segmentation_enabled->setChecked( + ihs_->getInputDataProcessing().getColorSegmentationEnabled()); } void -pcl::ihs::MainWindow::setXYZErodeSize (const int size) +pcl::ihs::MainWindow::setXYZErodeSize(const int size) { - ihs_->getInputDataProcessing ().setXYZErodeSize (static_cast (size)); - ui_->spinBox_xyz_erode_size->setValue (static_cast (ihs_->getInputDataProcessing ().getXYZErodeSize ())); + ihs_->getInputDataProcessing().setXYZErodeSize(static_cast(size)); + ui_->spinBox_xyz_erode_size->setValue( + static_cast(ihs_->getInputDataProcessing().getXYZErodeSize())); } void -pcl::ihs::MainWindow::setHSVDilateSize (const int size) +pcl::ihs::MainWindow::setHSVDilateSize(const int size) { - ihs_->getInputDataProcessing ().setHSVDilateSize (static_cast (size)); - ui_->spinBox_hsv_dilate_size->setValue (static_cast (ihs_->getInputDataProcessing ().getHSVDilateSize ())); + ihs_->getInputDataProcessing().setHSVDilateSize(static_cast(size)); + ui_->spinBox_hsv_dilate_size->setValue( + static_cast(ihs_->getInputDataProcessing().getHSVDilateSize())); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::setEpsilon () +pcl::ihs::MainWindow::setEpsilon() { - ihs_->getICP ().setEpsilon (ui_->lineEdit_epsilon->text ().toFloat ()); - ui_->lineEdit_epsilon->setText (QString ().setNum (ihs_->getICP ().getEpsilon ())); + ihs_->getICP().setEpsilon(ui_->lineEdit_epsilon->text().toFloat()); + ui_->lineEdit_epsilon->setText(QString().setNum(ihs_->getICP().getEpsilon())); } void -pcl::ihs::MainWindow::setMaxIterations (const int iterations) +pcl::ihs::MainWindow::setMaxIterations(const int iterations) { - ihs_->getICP ().setMaxIterations (static_cast (iterations)); - ui_->spinBox_max_iterations->setValue (static_cast (ihs_->getICP ().getMaxIterations ())); + ihs_->getICP().setMaxIterations(static_cast(iterations)); + ui_->spinBox_max_iterations->setValue( + static_cast(ihs_->getICP().getMaxIterations())); } void -pcl::ihs::MainWindow::setMinOverlap (const int overlap) +pcl::ihs::MainWindow::setMinOverlap(const int overlap) { - ihs_->getICP ().setMinOverlap (.01f * static_cast (overlap)); - ui_->spinBox_min_overlap->setValue (static_cast (100.f * ihs_->getICP ().getMinOverlap () + 0.5f)); + ihs_->getICP().setMinOverlap(.01f * static_cast(overlap)); + ui_->spinBox_min_overlap->setValue( + static_cast(100.f * ihs_->getICP().getMinOverlap() + 0.5f)); } void -pcl::ihs::MainWindow::setMaxFitness () +pcl::ihs::MainWindow::setMaxFitness() { - ihs_->getICP ().setMaxFitness (ui_->lineEdit_max_fitness->text ().toFloat ()); - ui_->lineEdit_max_fitness->setText (QString ().setNum (ihs_->getICP ().getMaxFitness ())); + ihs_->getICP().setMaxFitness(ui_->lineEdit_max_fitness->text().toFloat()); + ui_->lineEdit_max_fitness->setText(QString().setNum(ihs_->getICP().getMaxFitness())); } void -pcl::ihs::MainWindow::setCorrespondenceRejectionFactor (const double factor) +pcl::ihs::MainWindow::setCorrespondenceRejectionFactor(const double factor) { - ihs_->getICP ().setCorrespondenceRejectionFactor (static_cast (factor)); - ui_->doubleSpinBox_correspondence_rejection_factor->setValue (static_cast (ihs_->getICP ().getCorrespondenceRejectionFactor ())); + ihs_->getICP().setCorrespondenceRejectionFactor(static_cast(factor)); + ui_->doubleSpinBox_correspondence_rejection_factor->setValue( + static_cast(ihs_->getICP().getCorrespondenceRejectionFactor())); } void -pcl::ihs::MainWindow::setCorrespondenceRejectionMaxAngle (const int angle) +pcl::ihs::MainWindow::setCorrespondenceRejectionMaxAngle(const int angle) { - ihs_->getICP ().setMaxAngle (static_cast (angle)); - ui_->spinBox_correspondence_rejection_max_angle->setValue (static_cast (ihs_->getICP ().getMaxAngle ())); + ihs_->getICP().setMaxAngle(static_cast(angle)); + ui_->spinBox_correspondence_rejection_max_angle->setValue( + static_cast(ihs_->getICP().getMaxAngle())); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MainWindow::setMaxSquaredDistance () +pcl::ihs::MainWindow::setMaxSquaredDistance() { - ihs_->getIntegration ().setMaxSquaredDistance (ui_->lineEdit_max_squared_distance->text ().toFloat ()); - ui_->lineEdit_max_squared_distance->setText (QString ().setNum (ihs_->getIntegration ().getMaxSquaredDistance ())); + ihs_->getIntegration().setMaxSquaredDistance( + ui_->lineEdit_max_squared_distance->text().toFloat()); + ui_->lineEdit_max_squared_distance->setText( + QString().setNum(ihs_->getIntegration().getMaxSquaredDistance())); } void -pcl::ihs::MainWindow::setAveragingMaxAngle (const int angle) +pcl::ihs::MainWindow::setAveragingMaxAngle(const int angle) { - ihs_->getIntegration ().setMaxAngle (static_cast (angle)); - ui_->spinBox_averaging_max_angle->setValue (static_cast (ihs_->getIntegration ().getMaxAngle ())); + ihs_->getIntegration().setMaxAngle(static_cast(angle)); + ui_->spinBox_averaging_max_angle->setValue( + static_cast(ihs_->getIntegration().getMaxAngle())); } void -pcl::ihs::MainWindow::setMaxAge (const int age) +pcl::ihs::MainWindow::setMaxAge(const int age) { - ihs_->getIntegration ().setMaxAge (static_cast (age)); - ui_->spinBox_max_age->setValue (static_cast (ihs_->getIntegration ().getMaxAge ())); + ihs_->getIntegration().setMaxAge(static_cast(age)); + ui_->spinBox_max_age->setValue(static_cast(ihs_->getIntegration().getMaxAge())); } void -pcl::ihs::MainWindow::setMinDirections (const int directions) +pcl::ihs::MainWindow::setMinDirections(const int directions) { - ihs_->getIntegration ().setMinDirections (static_cast (directions)); - ui_->spinBox_min_directions->setValue (static_cast (ihs_->getIntegration ().getMinDirections ())); + ihs_->getIntegration().setMinDirections(static_cast(directions)); + ui_->spinBox_min_directions->setValue( + static_cast(ihs_->getIntegration().getMinDirections())); } //////////////////////////////////////////////////////////////////////////////// diff --git a/apps/in_hand_scanner/src/mesh_processing.cpp b/apps/in_hand_scanner/src/mesh_processing.cpp index feaf6f48..49701a48 100644 --- a/apps/in_hand_scanner/src/mesh_processing.cpp +++ b/apps/in_hand_scanner/src/mesh_processing.cpp @@ -39,49 +39,48 @@ */ #include +#include #include -#include - //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::MeshProcessing::MeshProcessing () -{ -} +pcl::ihs::MeshProcessing::MeshProcessing() = default; //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::MeshProcessing::processBoundary (Mesh& mesh, const std::vector & boundary_collection, const bool cleanup) const +pcl::ihs::MeshProcessing::processBoundary( + Mesh& mesh, + const std::vector& boundary_collection, + const bool cleanup) const { Mesh::VertexIndex vi_a, vi_b, vi_c, vi_d; Eigen::Vector3f ab, bc, ac, n_adb, n_plane; // Edges and normals Mesh::FaceIndex opposite_face; - for (const auto &boundary : boundary_collection) - { - if (boundary.size () == 3) - { - opposite_face = mesh.getOppositeFaceIndex (boundary [0]); + for (const auto& boundary : boundary_collection) { + if (boundary.size() == 3) { + opposite_face = mesh.getOppositeFaceIndex(boundary[0]); - if (mesh.getOppositeFaceIndex (boundary [1]) == opposite_face && - mesh.getOppositeFaceIndex (boundary [2]) == opposite_face) - { + if (mesh.getOppositeFaceIndex(boundary[1]) == opposite_face && + mesh.getOppositeFaceIndex(boundary[2]) == opposite_face) { // Isolated face. - mesh.deleteFace (opposite_face); + mesh.deleteFace(opposite_face); } - else - { + else { // Close triangular hole. - mesh.addFace (mesh.getTerminatingVertexIndex (boundary [0]), - mesh.getTerminatingVertexIndex (boundary [1]), - mesh.getTerminatingVertexIndex (boundary [2])); + mesh.addFace(mesh.getTerminatingVertexIndex(boundary[0]), + mesh.getTerminatingVertexIndex(boundary[1]), + mesh.getTerminatingVertexIndex(boundary[2])); } } else // size != 3 { - // Add triangles where the angle between the edges is below a threshold. In the example this would leave only triangles 1-2-3 and triangles 4-5-6 (threshold = 60 degrees). Triangle 1-2-3 should not be added because vertex 2 is not convex (as vertex 5). + // Add triangles where the angle between the edges is below a threshold. In the + // example this would leave only triangles 1-2-3 and triangles 4-5-6 (threshold = + // 60 degrees). Triangle 1-2-3 should not be added because vertex 2 is not convex + // (as vertex 5). // Example: The boundary is on the top. Vertex 7 is connected to vertex 0. // 2 // @@ -90,39 +89,42 @@ pcl::ihs::MeshProcessing::processBoundary (Mesh& mesh, const std::vector (); - bc = (v_c - v_b).head <3> (); - ac = (v_c - v_a).head <3> (); + ab = (v_b - v_a).head<3>(); + bc = (v_c - v_b).head<3>(); + ac = (v_c - v_a).head<3>(); - const float angle = std::acos (pcl::ihs::clamp (-ab.dot (bc) / ab.norm () / bc.norm (), -1.f, 1.f)); + const float angle = + std::acos(pcl::ihs::clamp(-ab.dot(bc) / ab.norm() / bc.norm(), -1.f, 1.f)); if (angle < 1.047197551196598f) // 60 * pi / 180 { // Third vertex belonging to the face of edge ab - vi_d = mesh.getTerminatingVertexIndex ( - mesh.getNextHalfEdgeIndex ( - mesh.getOppositeHalfEdgeIndex (boundary [i]))); - const Eigen::Vector4f& v_d = mesh.getVertexDataCloud () [vi_d.get ()].getVector4fMap (); + vi_d = mesh.getTerminatingVertexIndex( + mesh.getNextHalfEdgeIndex(mesh.getOppositeHalfEdgeIndex(boundary[i]))); + const Eigen::Vector4f& v_d = + mesh.getVertexDataCloud()[vi_d.get()].getVector4fMap(); // n_adb is the normal of triangle a-d-b. - // The plane goes through edge a-b and is perpendicular to the plane through a-d-b. - n_adb = (v_d - v_a).head <3> ().cross (ab)/*.normalized ()*/; - n_plane = n_adb.cross (ab/*.nomalized ()*/); + // The plane goes through edge a-b and is perpendicular to the plane through + // a-d-b. + n_adb = (v_d - v_a).head<3>().cross(ab) /*.normalized()*/; + n_plane = n_adb.cross(ab /*.nomalized()*/); - if (n_plane.dot (ac) > 0.f) - { - mesh.addFace (vi_a, vi_b, vi_c); + if (n_plane.dot(ac) > 0.f) { + mesh.addFace(vi_a, vi_b, vi_c); } } } @@ -130,7 +132,7 @@ pcl::ihs::MeshProcessing::processBoundary (Mesh& mesh, const std::vector #include +#include +#include +#include -#include -#include - -#include #include +#include #include #include -#include #include #include +#include -#include -#include -#include -#include +#include +#include //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::OfflineIntegration::OfflineIntegration (Base* parent) - : Base (parent), - mesh_model_ (new Mesh ()), - normal_estimation_ (new NormalEstimation ()), - integration_ (new Integration ()), - destructor_called_ (false) +pcl::ihs::OfflineIntegration::OfflineIntegration(Base* parent) +: Base(parent) +, mesh_model_(new Mesh()) +, normal_estimation_(new NormalEstimation()) +, integration_(new Integration()) +, destructor_called_(false) { - normal_estimation_->setNormalEstimationMethod (NormalEstimation::AVERAGE_3D_GRADIENT); - normal_estimation_->setMaxDepthChangeFactor (0.02f); // in meters - normal_estimation_->setNormalSmoothingSize (10.0f); - - integration_->setMaxSquaredDistance (1e-4); // in m^2 - integration_->setMinDirections (2); + normal_estimation_->setNormalEstimationMethod(NormalEstimation::AVERAGE_3D_GRADIENT); + normal_estimation_->setMaxDepthChangeFactor(0.02f); // in meters + normal_estimation_->setNormalSmoothingSize(10.0f); + integration_->setMaxSquaredDistance(1e-4); // in m^2 + integration_->setMinDirections(2); - Base::setVisibilityConfidenceNormalization (static_cast (integration_->getMinDirections ())); + Base::setVisibilityConfidenceNormalization( + static_cast(integration_->getMinDirections())); } //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::OfflineIntegration::~OfflineIntegration () +pcl::ihs::OfflineIntegration::~OfflineIntegration() { - std::lock_guard lock (mutex_); - std::lock_guard lock_quit (mutex_quit_); + std::lock_guard lock(mutex_); + std::lock_guard lock_quit(mutex_quit_); destructor_called_ = true; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OfflineIntegration::start () +pcl::ihs::OfflineIntegration::start() { - QString dir = QFileDialog::getExistingDirectory (nullptr, "Please select a directory containing .pcd and .transform files."); + QString dir = QFileDialog::getExistingDirectory( + nullptr, "Please select a directory containing .pcd and .transform files."); - if (dir.isEmpty ()) - { - return (QApplication::quit ()); + if (dir.isEmpty()) { + return (QApplication::quit()); } - path_dir_ = dir.toStdString (); - QtConcurrent::run ([this]{ computationThread (); }); + path_dir_ = dir.toStdString(); + QtConcurrent::run([this] { computationThread(); }); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OfflineIntegration::computationThread () +pcl::ihs::OfflineIntegration::computationThread() { - std::vector filenames; + std::vector filenames; - if (!this->getFilesFromDirectory (path_dir_, ".pcd", filenames)) - { - std::cerr << "ERROR in offline_integration.cpp: Could not get the files from the directory\n"; + if (!this->getFilesFromDirectory(path_dir_, ".pcd", filenames)) { + std::cerr << "ERROR in offline_integration.cpp: Could not get the files from the " + "directory\n"; return; } // First cloud is reference model - std::cerr << "Processing file " << std::setw (5) << 1 << " / " << filenames.size () << std::endl; - CloudXYZRGBNormalPtr cloud_model (new CloudXYZRGBNormal ()); - Eigen::Matrix4f T = Eigen::Matrix4f::Identity (); - if (!this->load (filenames [0], cloud_model, T)) - { + std::cerr << "Processing file " << std::setw(5) << 1 << " / " << filenames.size() + << std::endl; + CloudXYZRGBNormalPtr cloud_model(new CloudXYZRGBNormal()); + Eigen::Matrix4f T = Eigen::Matrix4f::Identity(); + if (!this->load(filenames[0], cloud_model, T)) { std::cerr << "ERROR in offline_integration.cpp: Could not load the model cloud.\n"; return; } - pcl::transformPointCloudWithNormals (*cloud_model, *cloud_model, T); + pcl::transformPointCloudWithNormals(*cloud_model, *cloud_model, T); - if (!integration_->reconstructMesh (cloud_model, mesh_model_)) - { - std::cerr << "ERROR in offline_integration.cpp: Could not reconstruct the model mesh.\n"; + if (!integration_->reconstructMesh(cloud_model, mesh_model_)) { + std::cerr + << "ERROR in offline_integration.cpp: Could not reconstruct the model mesh.\n"; return; } - Base::setPivot ("model"); - Base::addMesh (mesh_model_, "model"); + Base::setPivot("model"); + Base::addMesh(mesh_model_, "model"); - if (filenames.empty ()) - { + if (filenames.empty()) { return; } - for (std::size_t i=1; i lock (mutex_); - if (destructor_called_) return; + std::lock_guard lock(mutex_); + if (destructor_called_) + return; } - std::unique_lock lock_quit (mutex_quit_); + std::unique_lock lock_quit(mutex_quit_); - CloudXYZRGBNormalPtr cloud_data (new CloudXYZRGBNormal ()); - if (!this->load (filenames [i], cloud_data, T)) - { + CloudXYZRGBNormalPtr cloud_data(new CloudXYZRGBNormal()); + if (!this->load(filenames[i], cloud_data, T)) { std::cerr << "ERROR in offline_integration.cpp: Could not load the data cloud.\n"; return; } - if (!integration_->merge (cloud_data, mesh_model_, T)) - { + if (!integration_->merge(cloud_data, mesh_model_, T)) { std::cerr << "ERROR in offline_integration.cpp: merge failed.\n"; return; } - integration_->age (mesh_model_); + integration_->age(mesh_model_); { - lock_quit.unlock (); - std::lock_guard lock (mutex_); - if (destructor_called_) return; - - Base::addMesh (mesh_model_, "model", Eigen::Isometry3d (T.inverse ().cast ())); - Base::calcFPS (computation_fps_); + lock_quit.unlock(); + std::lock_guard lock(mutex_); + if (destructor_called_) + return; + + Base::addMesh( + mesh_model_, "model", Eigen::Isometry3d(T.inverse().cast())); + Base::calcFPS(computation_fps_); } } - Base::setPivot ("model"); + Base::setPivot("model"); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OfflineIntegration::getFilesFromDirectory (const std::string& path_dir, - const std::string& extension, - std::vector & files) const +pcl::ihs::OfflineIntegration::getFilesFromDirectory( + const std::string& path_dir, + const std::string& extension, + std::vector& files) const { - if (path_dir.empty() || !boost::filesystem::exists (path_dir)) - { - std::cerr << "ERROR in offline_integration.cpp: Invalid path\n '" << path_dir << "'\n"; + if (path_dir.empty() || !boost::filesystem::exists(path_dir)) { + std::cerr << "ERROR in offline_integration.cpp: Invalid path\n '" << path_dir + << "'\n"; return (false); } boost::filesystem::directory_iterator it_end; - for (boost::filesystem::directory_iterator it (path_dir); it != it_end; ++it) - { - if (!is_directory (it->status ()) && - boost::algorithm::to_upper_copy (boost::filesystem::extension (it->path ())) == boost::algorithm::to_upper_copy (extension)) - { - files.push_back (it->path ().string ()); + for (boost::filesystem::directory_iterator it(path_dir); it != it_end; ++it) { + if (!is_directory(it->status()) && + boost::algorithm::to_upper_copy(boost::filesystem::extension(it->path())) == + boost::algorithm::to_upper_copy(extension)) { + files.push_back(it->path().string()); } } - if (files.empty ()) - { - std::cerr << "ERROR in offline_integration.cpp: No '" << extension << "' files found\n"; + if (files.empty()) { + std::cerr << "ERROR in offline_integration.cpp: No '" << extension + << "' files found\n"; return (false); } - sort (files.begin (), files.end ()); + sort(files.begin(), files.end()); return (true); } @@ -215,82 +214,77 @@ pcl::ihs::OfflineIntegration::getFilesFromDirectory (const std::string& //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OfflineIntegration::loadTransform (const std::string& filename, - Eigen::Matrix4f& transform) const +pcl::ihs::OfflineIntegration::loadTransform(const std::string& filename, + Eigen::Matrix4f& transform) const { - Eigen::Matrix4d tr; - std::ifstream file; - file.open (filename.c_str (), std::ios::binary); - - if (!file.is_open ()) - { - std::cerr << "Error in offline_integration.cpp: Could not open the file '" << filename << "'\n"; - return (false); - } - - for (int i = 0; i < 4; ++i) - { - for (int j = 0; j < 4; ++j) - { - file.read (reinterpret_cast(&tr (i, j)), sizeof (double)); - - if (!file.good ()) - { - std::cerr << "Error in offline_integration.cpp: Could not read the transformation from the file.\n"; - return (false); - } - } - } - - transform = tr.cast (); - - return (true); + Eigen::Matrix4d tr; + std::ifstream file; + file.open(filename.c_str(), std::ios::binary); + + if (!file.is_open()) { + std::cerr << "Error in offline_integration.cpp: Could not open the file '" + << filename << "'\n"; + return (false); + } + + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + file.read(reinterpret_cast(&tr(i, j)), sizeof(double)); + + if (!file.good()) { + std::cerr << "Error in offline_integration.cpp: Could not read the " + "transformation from the file.\n"; + return (false); + } + } + } + + transform = tr.cast(); + + return (true); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OfflineIntegration::load (const std::string& filename, - CloudXYZRGBNormalPtr& cloud, - Eigen::Matrix4f& T) const +pcl::ihs::OfflineIntegration::load(const std::string& filename, + CloudXYZRGBNormalPtr& cloud, + Eigen::Matrix4f& T) const { - if (!cloud) - { - cloud = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ()); + if (!cloud) { + cloud = CloudXYZRGBNormalPtr(new CloudXYZRGBNormal()); } // Load the cloud. - CloudXYZRGBAPtr cloud_input (new CloudXYZRGBA ()); + CloudXYZRGBAPtr cloud_input(new CloudXYZRGBA()); pcl::PCDReader reader; - if (reader.read (filename, *cloud_input) < 0) - { + if (reader.read(filename, *cloud_input) < 0) { std::cerr << "ERROR in offline_integration.cpp: Could not read the pcd file.\n"; return (false); } // Normal estimation. - normal_estimation_->setInputCloud (cloud_input); - normal_estimation_->compute (*cloud); + normal_estimation_->setInputCloud(cloud_input); + normal_estimation_->compute(*cloud); - pcl::copyPointCloud (*cloud_input, *cloud); + pcl::copyPointCloud(*cloud_input, *cloud); // Change the file extension of the file. // Load the transformation. std::string fn_transform = filename; - std::size_t pos = fn_transform.find_last_of ('.'); - if (pos == std::string::npos || pos == (fn_transform.size () - 1)) - { + std::size_t pos = fn_transform.find_last_of('.'); + if (pos == std::string::npos || pos == (fn_transform.size() - 1)) { std::cerr << "ERROR in offline_integration.cpp: No file extension\n"; return (false); } - fn_transform.replace (pos, std::string::npos, ".transform"); + fn_transform.replace(pos, std::string::npos, ".transform"); - if (!this->loadTransform (fn_transform, T)) - { - std::cerr << "ERROR in offline_integration.cpp: Could not load the transformation.\n"; + if (!this->loadTransform(fn_transform, T)) { + std::cerr + << "ERROR in offline_integration.cpp: Could not load the transformation.\n"; return (false); } @@ -300,59 +294,69 @@ pcl::ihs::OfflineIntegration::load (const std::string& filename, //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OfflineIntegration::paintEvent (QPaintEvent* event) +pcl::ihs::OfflineIntegration::paintEvent(QPaintEvent* event) { - Base::paintEvent (event); + Base::paintEvent(event); - QPainter painter (this); - painter.setPen (Qt::white); + QPainter painter(this); + painter.setPen(Qt::white); QFont font; - font.setPointSize (this->width () / 50); - painter.setFont (font); + font.setPointSize(this->width() / 50); + painter.setFont(font); - std::string vis_fps ("Visualization: "), comp_fps ("Computation: "); + std::string vis_fps("Visualization: "), comp_fps("Computation: "); { - std::lock_guard lock (mutex_); - this->calcFPS (visualization_fps_); + std::lock_guard lock(mutex_); + this->calcFPS(visualization_fps_); - vis_fps.append (visualization_fps_.str ()).append (" fps"); - comp_fps.append (computation_fps_.str ()).append (" fps"); + vis_fps.append(visualization_fps_.str()).append(" fps"); + comp_fps.append(computation_fps_.str()).append(" fps"); } - const std::string str = std::string (comp_fps).append ("\n").append (vis_fps); + const std::string str = std::string(comp_fps).append("\n").append(vis_fps); - painter.drawText (0, 0, this->width (), this->height (), Qt::AlignBottom | Qt::AlignLeft, str.c_str ()); + painter.drawText(0, + 0, + this->width(), + this->height(), + Qt::AlignBottom | Qt::AlignLeft, + str.c_str()); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OfflineIntegration::keyPressEvent (QKeyEvent* event) +pcl::ihs::OfflineIntegration::keyPressEvent(QKeyEvent* event) { - if (event->key () == Qt::Key_Escape) - { - std::lock_guard lock (mutex_); - QApplication::quit (); + if (event->key() == Qt::Key_Escape) { + std::lock_guard lock(mutex_); + QApplication::quit(); } - switch (event->key ()) - { - case Qt::Key_H: - { - std::cerr << "======================================================================\n" - << "Help:\n" - << "----------------------------------------------------------------------\n" - << "ESC: Quit the application.\n" - << "c : Reset the camera.\n" - << "k : Toggle the coloring (rgb, one color, visibility-confidence).\n" - << "s : Toggle the mesh representation between points and faces.\n" - << "======================================================================\n"; - break; - } - case Qt::Key_C: Base::resetCamera (); break; - case Qt::Key_K: Base::toggleColoring (); break; - case Qt::Key_S: Base::toggleMeshRepresentation (); break; - default: break; + switch (event->key()) { + case Qt::Key_H: { + std::cerr + << "======================================================================\n" + << "Help:\n" + << "----------------------------------------------------------------------\n" + << "ESC: Quit the application.\n" + << "c : Reset the camera.\n" + << "k : Toggle the coloring (rgb, one color, visibility-confidence).\n" + << "s : Toggle the mesh representation between points and faces.\n" + << "======================================================================\n"; + break; + } + case Qt::Key_C: + Base::resetCamera(); + break; + case Qt::Key_K: + Base::toggleColoring(); + break; + case Qt::Key_S: + Base::toggleMeshRepresentation(); + break; + default: + break; } } diff --git a/apps/in_hand_scanner/src/opengl_viewer.cpp b/apps/in_hand_scanner/src/opengl_viewer.cpp index 9cdab712..054f601f 100644 --- a/apps/in_hand_scanner/src/opengl_viewer.cpp +++ b/apps/in_hand_scanner/src/opengl_viewer.cpp @@ -39,63 +39,60 @@ */ #include +#include #include -#include #include - -#include +#include #ifdef OPENGL_IS_A_FRAMEWORK -# include -# include +#include +#include #else -# include -# include +#include +#include #endif -#include - +#include #include #include // TODO: PointIHS is not registered -#include + +#include //////////////////////////////////////////////////////////////////////////////// // FaceVertexMesh //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh () - : transformation (Eigen::Isometry3d::Identity ()) -{ -} +pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh() +: transformation(Eigen::Isometry3d::Identity()) +{} //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh (const Mesh& mesh, const Eigen::Isometry3d& T) - : vertices (mesh.getVertexDataCloud ()), - transformation (T) +pcl::ihs::detail::FaceVertexMesh::FaceVertexMesh(const Mesh& mesh, + const Eigen::Isometry3d& T) +: vertices(mesh.getVertexDataCloud()), transformation(T) { - if (typeid (Mesh::MeshTag) != typeid (pcl::geometry::TriangleMeshTag)) - { - std::cerr << "In opengl_viewer.cpp: Only triangle meshes are currently supported!\n"; - exit (EXIT_FAILURE); + if (typeid(Mesh::MeshTag) != typeid(pcl::geometry::TriangleMeshTag)) { + std::cerr + << "In opengl_viewer.cpp: Only triangle meshes are currently supported!\n"; + exit(EXIT_FAILURE); } - for (auto &vertex : vertices) - { - std::swap (vertex.r, vertex.b); + for (auto& vertex : vertices) { + std::swap(vertex.r, vertex.b); } - triangles.reserve (mesh.sizeFaces ()); + triangles.reserve(mesh.sizeFaces()); pcl::ihs::detail::FaceVertexMesh::Triangle triangle; - for (std::size_t i=0; istart (33); + connect(timer_vis_.get(), SIGNAL(timeout()), this, SLOT(timerCallback())); + timer_vis_->start(33); // http://doc.qt.digia.com/qt/opengl-overpainting.html - QWidget::setAutoFillBackground (false); + QWidget::setAutoFillBackground(false); // http://doc.qt.digia.com/qt/qwidget.html#keyPressEvent - this->setFocusPolicy (Qt::StrongFocus); + this->setFocusPolicy(Qt::StrongFocus); // http://doc.qt.digia.com/qt/qmetatype.html#qRegisterMetaType - qRegisterMetaType ("MeshRepresentation"); - qRegisterMetaType ("Coloring"); + qRegisterMetaType("MeshRepresentation"); + qRegisterMetaType("Coloring"); ////////////////////////////////////////////////////////////////////////////// - // Code to generate the colormap (I don't want to link against vtk just for the colormap). + // Code to generate the colormap (I don't want to link against vtk just for the + // colormap). ////////////////////////////////////////////////////////////////////////////// //#include @@ -144,321 +142,320 @@ pcl::ihs::OpenGLViewer::OpenGLViewer (QWidget* parent) //#include //#include - //int - //main () - //{ - // static const unsigned int n = 256; - // // double rgb_1 [] = { 59./255., 76./255., 192./255.}; - // // double rgb_2 [] = {180./255., 4./255., 38./255.}; - // double rgb_1 [] = {180./255., 0./255., 0./255.}; - // double rgb_2 [] = { 0./255., 180./255., 0./255.}; - - // vtkSmartPointer ctf = vtkColorTransferFunction::New (); - // ctf->SetColorSpaceToDiverging (); - // ctf->AddRGBPoint ( 0., rgb_1 [0], rgb_1 [1], rgb_1 [2]); - // ctf->AddRGBPoint ( 1., rgb_2 [0], rgb_2 [1], rgb_2 [2]); - // ctf->Build (); - - // const unsigned char* colormap = ctf->GetTable (0., 1., n); - - // for (unsigned int i=0; i (colormap [3 * i ]); - // const unsigned int g = static_cast (colormap [3 * i + 1]); - // const unsigned int b = static_cast (colormap [3 * i + 2]); - - // std::cerr << "colormap_.col (" - // << std::setw (3) << i << ") = Color (" - // << std::setw (3) << r << ", " - // << std::setw (3) << g << ", " - // << std::setw (3) << b << ");\n"; - // } - - // return (EXIT_SUCCESS); - //} - - colormap_.col ( 0) = Color (180, 0, 0); - colormap_.col ( 1) = Color (182, 9, 1); - colormap_.col ( 2) = Color (184, 17, 1); - colormap_.col ( 3) = Color (186, 24, 2); - colormap_.col ( 4) = Color (188, 29, 2); - colormap_.col ( 5) = Color (190, 33, 3); - colormap_.col ( 6) = Color (192, 38, 4); - colormap_.col ( 7) = Color (194, 42, 5); - colormap_.col ( 8) = Color (196, 46, 6); - colormap_.col ( 9) = Color (197, 49, 7); - colormap_.col ( 10) = Color (199, 53, 9); - colormap_.col ( 11) = Color (201, 56, 10); - colormap_.col ( 12) = Color (203, 59, 12); - colormap_.col ( 13) = Color (205, 63, 13); - colormap_.col ( 14) = Color (207, 66, 15); - colormap_.col ( 15) = Color (208, 69, 17); - colormap_.col ( 16) = Color (210, 72, 18); - colormap_.col ( 17) = Color (212, 75, 20); - colormap_.col ( 18) = Color (214, 78, 21); - colormap_.col ( 19) = Color (215, 81, 23); - colormap_.col ( 20) = Color (217, 84, 25); - colormap_.col ( 21) = Color (219, 87, 26); - colormap_.col ( 22) = Color (221, 89, 28); - colormap_.col ( 23) = Color (222, 92, 30); - colormap_.col ( 24) = Color (224, 95, 32); - colormap_.col ( 25) = Color (225, 98, 33); - colormap_.col ( 26) = Color (227, 101, 35); - colormap_.col ( 27) = Color (229, 103, 37); - colormap_.col ( 28) = Color (230, 106, 39); - colormap_.col ( 29) = Color (232, 109, 40); - colormap_.col ( 30) = Color (233, 112, 42); - colormap_.col ( 31) = Color (235, 114, 44); - colormap_.col ( 32) = Color (236, 117, 46); - colormap_.col ( 33) = Color (238, 120, 48); - colormap_.col ( 34) = Color (239, 122, 50); - colormap_.col ( 35) = Color (241, 125, 52); - colormap_.col ( 36) = Color (242, 127, 54); - colormap_.col ( 37) = Color (244, 130, 56); - colormap_.col ( 38) = Color (245, 133, 58); - colormap_.col ( 39) = Color (246, 135, 60); - colormap_.col ( 40) = Color (248, 138, 62); - colormap_.col ( 41) = Color (249, 140, 64); - colormap_.col ( 42) = Color (250, 143, 66); - colormap_.col ( 43) = Color (252, 145, 68); - colormap_.col ( 44) = Color (253, 148, 70); - colormap_.col ( 45) = Color (254, 150, 73); - colormap_.col ( 46) = Color (255, 153, 75); - colormap_.col ( 47) = Color (255, 154, 76); - colormap_.col ( 48) = Color (255, 156, 78); - colormap_.col ( 49) = Color (255, 158, 80); - colormap_.col ( 50) = Color (255, 159, 82); - colormap_.col ( 51) = Color (255, 161, 84); - colormap_.col ( 52) = Color (255, 163, 86); - colormap_.col ( 53) = Color (255, 164, 88); - colormap_.col ( 54) = Color (255, 166, 90); - colormap_.col ( 55) = Color (255, 168, 92); - colormap_.col ( 56) = Color (255, 169, 93); - colormap_.col ( 57) = Color (255, 171, 95); - colormap_.col ( 58) = Color (255, 172, 97); - colormap_.col ( 59) = Color (255, 174, 99); - colormap_.col ( 60) = Color (255, 176, 101); - colormap_.col ( 61) = Color (255, 177, 103); - colormap_.col ( 62) = Color (255, 179, 105); - colormap_.col ( 63) = Color (255, 180, 107); - colormap_.col ( 64) = Color (255, 182, 109); - colormap_.col ( 65) = Color (255, 183, 111); - colormap_.col ( 66) = Color (255, 185, 113); - colormap_.col ( 67) = Color (255, 186, 115); - colormap_.col ( 68) = Color (255, 188, 117); - colormap_.col ( 69) = Color (255, 189, 119); - colormap_.col ( 70) = Color (255, 191, 122); - colormap_.col ( 71) = Color (255, 192, 124); - colormap_.col ( 72) = Color (255, 194, 126); - colormap_.col ( 73) = Color (255, 195, 128); - colormap_.col ( 74) = Color (255, 196, 130); - colormap_.col ( 75) = Color (255, 198, 132); - colormap_.col ( 76) = Color (255, 199, 134); - colormap_.col ( 77) = Color (255, 201, 136); - colormap_.col ( 78) = Color (255, 202, 139); - colormap_.col ( 79) = Color (255, 203, 141); - colormap_.col ( 80) = Color (255, 205, 143); - colormap_.col ( 81) = Color (255, 206, 145); - colormap_.col ( 82) = Color (255, 207, 147); - colormap_.col ( 83) = Color (255, 209, 149); - colormap_.col ( 84) = Color (255, 210, 152); - colormap_.col ( 85) = Color (255, 211, 154); - colormap_.col ( 86) = Color (255, 213, 156); - colormap_.col ( 87) = Color (255, 214, 158); - colormap_.col ( 88) = Color (255, 215, 161); - colormap_.col ( 89) = Color (255, 216, 163); - colormap_.col ( 90) = Color (255, 218, 165); - colormap_.col ( 91) = Color (255, 219, 168); - colormap_.col ( 92) = Color (255, 220, 170); - colormap_.col ( 93) = Color (255, 221, 172); - colormap_.col ( 94) = Color (255, 223, 175); - colormap_.col ( 95) = Color (255, 224, 177); - colormap_.col ( 96) = Color (255, 225, 179); - colormap_.col ( 97) = Color (255, 226, 182); - colormap_.col ( 98) = Color (255, 227, 184); - colormap_.col ( 99) = Color (255, 228, 186); - colormap_.col (100) = Color (255, 230, 189); - colormap_.col (101) = Color (255, 231, 191); - colormap_.col (102) = Color (255, 232, 193); - colormap_.col (103) = Color (255, 233, 196); - colormap_.col (104) = Color (255, 234, 198); - colormap_.col (105) = Color (255, 235, 201); - colormap_.col (106) = Color (255, 236, 203); - colormap_.col (107) = Color (255, 237, 205); - colormap_.col (108) = Color (255, 238, 208); - colormap_.col (109) = Color (255, 239, 210); - colormap_.col (110) = Color (255, 240, 213); - colormap_.col (111) = Color (255, 241, 215); - colormap_.col (112) = Color (255, 242, 218); - colormap_.col (113) = Color (255, 243, 220); - colormap_.col (114) = Color (255, 244, 222); - colormap_.col (115) = Color (255, 245, 225); - colormap_.col (116) = Color (255, 246, 227); - colormap_.col (117) = Color (255, 247, 230); - colormap_.col (118) = Color (255, 248, 232); - colormap_.col (119) = Color (255, 249, 235); - colormap_.col (120) = Color (255, 249, 237); - colormap_.col (121) = Color (255, 250, 239); - colormap_.col (122) = Color (255, 251, 242); - colormap_.col (123) = Color (255, 252, 244); - colormap_.col (124) = Color (255, 253, 247); - colormap_.col (125) = Color (255, 253, 249); - colormap_.col (126) = Color (255, 254, 251); - colormap_.col (127) = Color (255, 255, 254); - colormap_.col (128) = Color (255, 255, 254); - colormap_.col (129) = Color (254, 255, 253); - colormap_.col (130) = Color (253, 255, 252); - colormap_.col (131) = Color (252, 255, 250); - colormap_.col (132) = Color (251, 255, 249); - colormap_.col (133) = Color (250, 255, 248); - colormap_.col (134) = Color (249, 255, 246); - colormap_.col (135) = Color (248, 255, 245); - colormap_.col (136) = Color (247, 255, 244); - colormap_.col (137) = Color (246, 255, 242); - colormap_.col (138) = Color (245, 255, 241); - colormap_.col (139) = Color (244, 255, 240); - colormap_.col (140) = Color (243, 255, 238); - colormap_.col (141) = Color (242, 255, 237); - colormap_.col (142) = Color (241, 255, 236); - colormap_.col (143) = Color (240, 255, 235); - colormap_.col (144) = Color (239, 255, 233); - colormap_.col (145) = Color (238, 255, 232); - colormap_.col (146) = Color (237, 255, 231); - colormap_.col (147) = Color (236, 255, 229); - colormap_.col (148) = Color (235, 255, 228); - colormap_.col (149) = Color (234, 255, 227); - colormap_.col (150) = Color (234, 255, 225); - colormap_.col (151) = Color (233, 255, 224); - colormap_.col (152) = Color (232, 255, 223); - colormap_.col (153) = Color (231, 255, 221); - colormap_.col (154) = Color (230, 255, 220); - colormap_.col (155) = Color (229, 255, 219); - colormap_.col (156) = Color (228, 255, 218); - colormap_.col (157) = Color (227, 255, 216); - colormap_.col (158) = Color (226, 255, 215); - colormap_.col (159) = Color (225, 255, 214); - colormap_.col (160) = Color (224, 255, 212); - colormap_.col (161) = Color (223, 255, 211); - colormap_.col (162) = Color (222, 255, 210); - colormap_.col (163) = Color (221, 255, 208); - colormap_.col (164) = Color (220, 255, 207); - colormap_.col (165) = Color (219, 255, 206); - colormap_.col (166) = Color (218, 255, 204); - colormap_.col (167) = Color (217, 255, 203); - colormap_.col (168) = Color (216, 255, 202); - colormap_.col (169) = Color (215, 255, 201); - colormap_.col (170) = Color (214, 255, 199); - colormap_.col (171) = Color (213, 255, 198); - colormap_.col (172) = Color (211, 255, 197); - colormap_.col (173) = Color (210, 255, 195); - colormap_.col (174) = Color (209, 255, 194); - colormap_.col (175) = Color (208, 255, 193); - colormap_.col (176) = Color (207, 255, 191); - colormap_.col (177) = Color (206, 255, 190); - colormap_.col (178) = Color (205, 255, 188); - colormap_.col (179) = Color (204, 255, 187); - colormap_.col (180) = Color (203, 255, 186); - colormap_.col (181) = Color (202, 255, 184); - colormap_.col (182) = Color (201, 255, 183); - colormap_.col (183) = Color (199, 255, 182); - colormap_.col (184) = Color (198, 255, 180); - colormap_.col (185) = Color (197, 255, 179); - colormap_.col (186) = Color (196, 255, 177); - colormap_.col (187) = Color (195, 255, 176); - colormap_.col (188) = Color (194, 255, 174); - colormap_.col (189) = Color (192, 255, 173); - colormap_.col (190) = Color (191, 255, 172); - colormap_.col (191) = Color (190, 255, 170); - colormap_.col (192) = Color (189, 255, 169); - colormap_.col (193) = Color (188, 255, 167); - colormap_.col (194) = Color (186, 255, 166); - colormap_.col (195) = Color (185, 255, 164); - colormap_.col (196) = Color (184, 255, 163); - colormap_.col (197) = Color (183, 255, 161); - colormap_.col (198) = Color (181, 255, 160); - colormap_.col (199) = Color (180, 255, 158); - colormap_.col (200) = Color (179, 255, 157); - colormap_.col (201) = Color (177, 255, 155); - colormap_.col (202) = Color (176, 255, 154); - colormap_.col (203) = Color (175, 255, 152); - colormap_.col (204) = Color (173, 255, 150); - colormap_.col (205) = Color (172, 255, 149); - colormap_.col (206) = Color (170, 255, 147); - colormap_.col (207) = Color (169, 255, 145); - colormap_.col (208) = Color (166, 253, 143); - colormap_.col (209) = Color (164, 252, 141); - colormap_.col (210) = Color (162, 251, 138); - colormap_.col (211) = Color (159, 250, 136); - colormap_.col (212) = Color (157, 248, 134); - colormap_.col (213) = Color (155, 247, 131); - colormap_.col (214) = Color (152, 246, 129); - colormap_.col (215) = Color (150, 245, 127); - colormap_.col (216) = Color (148, 243, 124); - colormap_.col (217) = Color (145, 242, 122); - colormap_.col (218) = Color (143, 240, 119); - colormap_.col (219) = Color (140, 239, 117); - colormap_.col (220) = Color (138, 238, 114); - colormap_.col (221) = Color (135, 236, 112); - colormap_.col (222) = Color (133, 235, 110); - colormap_.col (223) = Color (130, 233, 107); - colormap_.col (224) = Color (128, 232, 105); - colormap_.col (225) = Color (125, 230, 102); - colormap_.col (226) = Color (122, 229, 100); - colormap_.col (227) = Color (120, 227, 97); - colormap_.col (228) = Color (117, 226, 94); - colormap_.col (229) = Color (114, 224, 92); - colormap_.col (230) = Color (111, 223, 89); - colormap_.col (231) = Color (109, 221, 87); - colormap_.col (232) = Color (106, 220, 84); - colormap_.col (233) = Color (103, 218, 82); - colormap_.col (234) = Color (100, 217, 79); - colormap_.col (235) = Color ( 97, 215, 76); - colormap_.col (236) = Color ( 94, 213, 73); - colormap_.col (237) = Color ( 91, 212, 71); - colormap_.col (238) = Color ( 88, 210, 68); - colormap_.col (239) = Color ( 85, 208, 65); - colormap_.col (240) = Color ( 82, 207, 62); - colormap_.col (241) = Color ( 78, 205, 59); - colormap_.col (242) = Color ( 75, 203, 57); - colormap_.col (243) = Color ( 71, 201, 54); - colormap_.col (244) = Color ( 68, 200, 50); - colormap_.col (245) = Color ( 64, 198, 47); - colormap_.col (246) = Color ( 60, 196, 44); - colormap_.col (247) = Color ( 56, 195, 41); - colormap_.col (248) = Color ( 52, 193, 37); - colormap_.col (249) = Color ( 47, 191, 33); - colormap_.col (250) = Color ( 42, 189, 29); - colormap_.col (251) = Color ( 37, 187, 25); - colormap_.col (252) = Color ( 31, 186, 20); - colormap_.col (253) = Color ( 24, 184, 15); - colormap_.col (254) = Color ( 14, 182, 7); - colormap_.col (255) = Color ( 0, 180, 0); + // int + // main() + // { + // static const unsigned int n = 256; + // // double rgb_1 [] = { 59./255., 76./255., 192./255.}; + // // double rgb_2 [] = {180./255., 4./255., 38./255.}; + // double rgb_1 [] = {180./255., 0./255., 0./255.}; + // double rgb_2 [] = { 0./255., 180./255., 0./255.}; + // + // vtkSmartPointer ctf = vtkColorTransferFunction::New(); + // ctf->SetColorSpaceToDiverging(); + // ctf->AddRGBPoint(0., rgb_1 [0], rgb_1 [1], rgb_1 [2]); + // ctf->AddRGBPoint(1., rgb_2 [0], rgb_2 [1], rgb_2 [2]); + // ctf->Build(); + // + // const unsigned char* colormap = ctf->GetTable (0., 1., n); + // + // for (unsigned int i=0; i (colormap [3 * i ]); + // const unsigned int g = static_cast (colormap [3 * i + 1]); + // const unsigned int b = static_cast (colormap [3 * i + 2]); + // + // std::cerr << "colormap_.col (" + // << std::setw (3) << i << ") = Color (" + // << std::setw (3) << r << ", " + // << std::setw (3) << g << ", " + // << std::setw (3) << b << ");\n"; + // } + // + // return (EXIT_SUCCESS); + // } + + colormap_.col(0) = Color(180, 0, 0); + colormap_.col(1) = Color(182, 9, 1); + colormap_.col(2) = Color(184, 17, 1); + colormap_.col(3) = Color(186, 24, 2); + colormap_.col(4) = Color(188, 29, 2); + colormap_.col(5) = Color(190, 33, 3); + colormap_.col(6) = Color(192, 38, 4); + colormap_.col(7) = Color(194, 42, 5); + colormap_.col(8) = Color(196, 46, 6); + colormap_.col(9) = Color(197, 49, 7); + colormap_.col(10) = Color(199, 53, 9); + colormap_.col(11) = Color(201, 56, 10); + colormap_.col(12) = Color(203, 59, 12); + colormap_.col(13) = Color(205, 63, 13); + colormap_.col(14) = Color(207, 66, 15); + colormap_.col(15) = Color(208, 69, 17); + colormap_.col(16) = Color(210, 72, 18); + colormap_.col(17) = Color(212, 75, 20); + colormap_.col(18) = Color(214, 78, 21); + colormap_.col(19) = Color(215, 81, 23); + colormap_.col(20) = Color(217, 84, 25); + colormap_.col(21) = Color(219, 87, 26); + colormap_.col(22) = Color(221, 89, 28); + colormap_.col(23) = Color(222, 92, 30); + colormap_.col(24) = Color(224, 95, 32); + colormap_.col(25) = Color(225, 98, 33); + colormap_.col(26) = Color(227, 101, 35); + colormap_.col(27) = Color(229, 103, 37); + colormap_.col(28) = Color(230, 106, 39); + colormap_.col(29) = Color(232, 109, 40); + colormap_.col(30) = Color(233, 112, 42); + colormap_.col(31) = Color(235, 114, 44); + colormap_.col(32) = Color(236, 117, 46); + colormap_.col(33) = Color(238, 120, 48); + colormap_.col(34) = Color(239, 122, 50); + colormap_.col(35) = Color(241, 125, 52); + colormap_.col(36) = Color(242, 127, 54); + colormap_.col(37) = Color(244, 130, 56); + colormap_.col(38) = Color(245, 133, 58); + colormap_.col(39) = Color(246, 135, 60); + colormap_.col(40) = Color(248, 138, 62); + colormap_.col(41) = Color(249, 140, 64); + colormap_.col(42) = Color(250, 143, 66); + colormap_.col(43) = Color(252, 145, 68); + colormap_.col(44) = Color(253, 148, 70); + colormap_.col(45) = Color(254, 150, 73); + colormap_.col(46) = Color(255, 153, 75); + colormap_.col(47) = Color(255, 154, 76); + colormap_.col(48) = Color(255, 156, 78); + colormap_.col(49) = Color(255, 158, 80); + colormap_.col(50) = Color(255, 159, 82); + colormap_.col(51) = Color(255, 161, 84); + colormap_.col(52) = Color(255, 163, 86); + colormap_.col(53) = Color(255, 164, 88); + colormap_.col(54) = Color(255, 166, 90); + colormap_.col(55) = Color(255, 168, 92); + colormap_.col(56) = Color(255, 169, 93); + colormap_.col(57) = Color(255, 171, 95); + colormap_.col(58) = Color(255, 172, 97); + colormap_.col(59) = Color(255, 174, 99); + colormap_.col(60) = Color(255, 176, 101); + colormap_.col(61) = Color(255, 177, 103); + colormap_.col(62) = Color(255, 179, 105); + colormap_.col(63) = Color(255, 180, 107); + colormap_.col(64) = Color(255, 182, 109); + colormap_.col(65) = Color(255, 183, 111); + colormap_.col(66) = Color(255, 185, 113); + colormap_.col(67) = Color(255, 186, 115); + colormap_.col(68) = Color(255, 188, 117); + colormap_.col(69) = Color(255, 189, 119); + colormap_.col(70) = Color(255, 191, 122); + colormap_.col(71) = Color(255, 192, 124); + colormap_.col(72) = Color(255, 194, 126); + colormap_.col(73) = Color(255, 195, 128); + colormap_.col(74) = Color(255, 196, 130); + colormap_.col(75) = Color(255, 198, 132); + colormap_.col(76) = Color(255, 199, 134); + colormap_.col(77) = Color(255, 201, 136); + colormap_.col(78) = Color(255, 202, 139); + colormap_.col(79) = Color(255, 203, 141); + colormap_.col(80) = Color(255, 205, 143); + colormap_.col(81) = Color(255, 206, 145); + colormap_.col(82) = Color(255, 207, 147); + colormap_.col(83) = Color(255, 209, 149); + colormap_.col(84) = Color(255, 210, 152); + colormap_.col(85) = Color(255, 211, 154); + colormap_.col(86) = Color(255, 213, 156); + colormap_.col(87) = Color(255, 214, 158); + colormap_.col(88) = Color(255, 215, 161); + colormap_.col(89) = Color(255, 216, 163); + colormap_.col(90) = Color(255, 218, 165); + colormap_.col(91) = Color(255, 219, 168); + colormap_.col(92) = Color(255, 220, 170); + colormap_.col(93) = Color(255, 221, 172); + colormap_.col(94) = Color(255, 223, 175); + colormap_.col(95) = Color(255, 224, 177); + colormap_.col(96) = Color(255, 225, 179); + colormap_.col(97) = Color(255, 226, 182); + colormap_.col(98) = Color(255, 227, 184); + colormap_.col(99) = Color(255, 228, 186); + colormap_.col(100) = Color(255, 230, 189); + colormap_.col(101) = Color(255, 231, 191); + colormap_.col(102) = Color(255, 232, 193); + colormap_.col(103) = Color(255, 233, 196); + colormap_.col(104) = Color(255, 234, 198); + colormap_.col(105) = Color(255, 235, 201); + colormap_.col(106) = Color(255, 236, 203); + colormap_.col(107) = Color(255, 237, 205); + colormap_.col(108) = Color(255, 238, 208); + colormap_.col(109) = Color(255, 239, 210); + colormap_.col(110) = Color(255, 240, 213); + colormap_.col(111) = Color(255, 241, 215); + colormap_.col(112) = Color(255, 242, 218); + colormap_.col(113) = Color(255, 243, 220); + colormap_.col(114) = Color(255, 244, 222); + colormap_.col(115) = Color(255, 245, 225); + colormap_.col(116) = Color(255, 246, 227); + colormap_.col(117) = Color(255, 247, 230); + colormap_.col(118) = Color(255, 248, 232); + colormap_.col(119) = Color(255, 249, 235); + colormap_.col(120) = Color(255, 249, 237); + colormap_.col(121) = Color(255, 250, 239); + colormap_.col(122) = Color(255, 251, 242); + colormap_.col(123) = Color(255, 252, 244); + colormap_.col(124) = Color(255, 253, 247); + colormap_.col(125) = Color(255, 253, 249); + colormap_.col(126) = Color(255, 254, 251); + colormap_.col(127) = Color(255, 255, 254); + colormap_.col(128) = Color(255, 255, 254); + colormap_.col(129) = Color(254, 255, 253); + colormap_.col(130) = Color(253, 255, 252); + colormap_.col(131) = Color(252, 255, 250); + colormap_.col(132) = Color(251, 255, 249); + colormap_.col(133) = Color(250, 255, 248); + colormap_.col(134) = Color(249, 255, 246); + colormap_.col(135) = Color(248, 255, 245); + colormap_.col(136) = Color(247, 255, 244); + colormap_.col(137) = Color(246, 255, 242); + colormap_.col(138) = Color(245, 255, 241); + colormap_.col(139) = Color(244, 255, 240); + colormap_.col(140) = Color(243, 255, 238); + colormap_.col(141) = Color(242, 255, 237); + colormap_.col(142) = Color(241, 255, 236); + colormap_.col(143) = Color(240, 255, 235); + colormap_.col(144) = Color(239, 255, 233); + colormap_.col(145) = Color(238, 255, 232); + colormap_.col(146) = Color(237, 255, 231); + colormap_.col(147) = Color(236, 255, 229); + colormap_.col(148) = Color(235, 255, 228); + colormap_.col(149) = Color(234, 255, 227); + colormap_.col(150) = Color(234, 255, 225); + colormap_.col(151) = Color(233, 255, 224); + colormap_.col(152) = Color(232, 255, 223); + colormap_.col(153) = Color(231, 255, 221); + colormap_.col(154) = Color(230, 255, 220); + colormap_.col(155) = Color(229, 255, 219); + colormap_.col(156) = Color(228, 255, 218); + colormap_.col(157) = Color(227, 255, 216); + colormap_.col(158) = Color(226, 255, 215); + colormap_.col(159) = Color(225, 255, 214); + colormap_.col(160) = Color(224, 255, 212); + colormap_.col(161) = Color(223, 255, 211); + colormap_.col(162) = Color(222, 255, 210); + colormap_.col(163) = Color(221, 255, 208); + colormap_.col(164) = Color(220, 255, 207); + colormap_.col(165) = Color(219, 255, 206); + colormap_.col(166) = Color(218, 255, 204); + colormap_.col(167) = Color(217, 255, 203); + colormap_.col(168) = Color(216, 255, 202); + colormap_.col(169) = Color(215, 255, 201); + colormap_.col(170) = Color(214, 255, 199); + colormap_.col(171) = Color(213, 255, 198); + colormap_.col(172) = Color(211, 255, 197); + colormap_.col(173) = Color(210, 255, 195); + colormap_.col(174) = Color(209, 255, 194); + colormap_.col(175) = Color(208, 255, 193); + colormap_.col(176) = Color(207, 255, 191); + colormap_.col(177) = Color(206, 255, 190); + colormap_.col(178) = Color(205, 255, 188); + colormap_.col(179) = Color(204, 255, 187); + colormap_.col(180) = Color(203, 255, 186); + colormap_.col(181) = Color(202, 255, 184); + colormap_.col(182) = Color(201, 255, 183); + colormap_.col(183) = Color(199, 255, 182); + colormap_.col(184) = Color(198, 255, 180); + colormap_.col(185) = Color(197, 255, 179); + colormap_.col(186) = Color(196, 255, 177); + colormap_.col(187) = Color(195, 255, 176); + colormap_.col(188) = Color(194, 255, 174); + colormap_.col(189) = Color(192, 255, 173); + colormap_.col(190) = Color(191, 255, 172); + colormap_.col(191) = Color(190, 255, 170); + colormap_.col(192) = Color(189, 255, 169); + colormap_.col(193) = Color(188, 255, 167); + colormap_.col(194) = Color(186, 255, 166); + colormap_.col(195) = Color(185, 255, 164); + colormap_.col(196) = Color(184, 255, 163); + colormap_.col(197) = Color(183, 255, 161); + colormap_.col(198) = Color(181, 255, 160); + colormap_.col(199) = Color(180, 255, 158); + colormap_.col(200) = Color(179, 255, 157); + colormap_.col(201) = Color(177, 255, 155); + colormap_.col(202) = Color(176, 255, 154); + colormap_.col(203) = Color(175, 255, 152); + colormap_.col(204) = Color(173, 255, 150); + colormap_.col(205) = Color(172, 255, 149); + colormap_.col(206) = Color(170, 255, 147); + colormap_.col(207) = Color(169, 255, 145); + colormap_.col(208) = Color(166, 253, 143); + colormap_.col(209) = Color(164, 252, 141); + colormap_.col(210) = Color(162, 251, 138); + colormap_.col(211) = Color(159, 250, 136); + colormap_.col(212) = Color(157, 248, 134); + colormap_.col(213) = Color(155, 247, 131); + colormap_.col(214) = Color(152, 246, 129); + colormap_.col(215) = Color(150, 245, 127); + colormap_.col(216) = Color(148, 243, 124); + colormap_.col(217) = Color(145, 242, 122); + colormap_.col(218) = Color(143, 240, 119); + colormap_.col(219) = Color(140, 239, 117); + colormap_.col(220) = Color(138, 238, 114); + colormap_.col(221) = Color(135, 236, 112); + colormap_.col(222) = Color(133, 235, 110); + colormap_.col(223) = Color(130, 233, 107); + colormap_.col(224) = Color(128, 232, 105); + colormap_.col(225) = Color(125, 230, 102); + colormap_.col(226) = Color(122, 229, 100); + colormap_.col(227) = Color(120, 227, 97); + colormap_.col(228) = Color(117, 226, 94); + colormap_.col(229) = Color(114, 224, 92); + colormap_.col(230) = Color(111, 223, 89); + colormap_.col(231) = Color(109, 221, 87); + colormap_.col(232) = Color(106, 220, 84); + colormap_.col(233) = Color(103, 218, 82); + colormap_.col(234) = Color(100, 217, 79); + colormap_.col(235) = Color(97, 215, 76); + colormap_.col(236) = Color(94, 213, 73); + colormap_.col(237) = Color(91, 212, 71); + colormap_.col(238) = Color(88, 210, 68); + colormap_.col(239) = Color(85, 208, 65); + colormap_.col(240) = Color(82, 207, 62); + colormap_.col(241) = Color(78, 205, 59); + colormap_.col(242) = Color(75, 203, 57); + colormap_.col(243) = Color(71, 201, 54); + colormap_.col(244) = Color(68, 200, 50); + colormap_.col(245) = Color(64, 198, 47); + colormap_.col(246) = Color(60, 196, 44); + colormap_.col(247) = Color(56, 195, 41); + colormap_.col(248) = Color(52, 193, 37); + colormap_.col(249) = Color(47, 191, 33); + colormap_.col(250) = Color(42, 189, 29); + colormap_.col(251) = Color(37, 187, 25); + colormap_.col(252) = Color(31, 186, 20); + colormap_.col(253) = Color(24, 184, 15); + colormap_.col(254) = Color(14, 182, 7); + colormap_.col(255) = Color(0, 180, 0); } //////////////////////////////////////////////////////////////////////////////// -pcl::ihs::OpenGLViewer::~OpenGLViewer () -{ - this->stopTimer (); -} +pcl::ihs::OpenGLViewer::~OpenGLViewer() { this->stopTimer(); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OpenGLViewer::addMesh (const MeshConstPtr& mesh, const std::string& id, const Eigen::Isometry3d& T) +pcl::ihs::OpenGLViewer::addMesh(const MeshConstPtr& mesh, + const std::string& id, + const Eigen::Isometry3d& T) { - if (!mesh) - { + if (!mesh) { std::cerr << "ERROR in opengl_viewer.cpp: Input mesh is not valid.\n"; return (false); } - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); - if (this->getMeshIsAdded (id)) - drawn_meshes_ [id] = FaceVertexMeshPtr (new FaceVertexMesh (*mesh, T)); + if (this->getMeshIsAdded(id)) + drawn_meshes_[id] = FaceVertexMeshPtr(new FaceVertexMesh(*mesh, T)); else - drawn_meshes_.insert (std::make_pair (id, FaceVertexMeshPtr (new FaceVertexMesh (*mesh, T)))); + drawn_meshes_.insert( + std::make_pair(id, FaceVertexMeshPtr(new FaceVertexMesh(*mesh, T)))); return (true); } @@ -466,15 +463,15 @@ pcl::ihs::OpenGLViewer::addMesh (const MeshConstPtr& mesh, const std::string& id //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OpenGLViewer::addMesh (const CloudXYZRGBNormalConstPtr& cloud, const std::string& id, const Eigen::Isometry3d& T) +pcl::ihs::OpenGLViewer::addMesh(const CloudXYZRGBNormalConstPtr& cloud, + const std::string& id, + const Eigen::Isometry3d& T) { - if (!cloud) - { + if (!cloud) { std::cerr << "ERROR in opengl_viewer.cpp: Input cloud is not valid.\n"; return (false); } - if (!cloud->isOrganized ()) - { + if (!cloud->isOrganized()) { std::cerr << "ERROR in opengl_viewer.cpp: Input cloud is not organized.\n"; return (false); } @@ -483,31 +480,31 @@ pcl::ihs::OpenGLViewer::addMesh (const CloudXYZRGBNormalConstPtr& cloud, const s // 2 - 1 // // | / | // // 3 - 0 // - const int w = cloud->width; - const int h = cloud->height; + const int w = cloud->width; + const int h = cloud->height; const int offset_1 = -w; const int offset_2 = -w - 1; - const int offset_3 = - 1; + const int offset_3 = -1; - FaceVertexMeshPtr mesh (new FaceVertexMesh ()); + FaceVertexMeshPtr mesh(new FaceVertexMesh()); mesh->transformation = T; - std::vector indices (w * h, -1); // Map the original indices to the vertex indices. + std::vector indices(w * h, + -1); // Map the original indices to the vertex indices. CloudIHS& vertices = mesh->vertices; - std::vector & triangles = mesh->triangles; - vertices.reserve (cloud->size ()); - triangles.reserve (2 * (w-1) * (h-1)); + std::vector& triangles = mesh->triangles; + vertices.reserve(cloud->size()); + triangles.reserve(2 * (w - 1) * (h - 1)); // Helper functor - struct AddVertex - { - inline int operator () (const PointXYZRGBNormal& pt, CloudIHS& vertices, int& ind_o) const + struct AddVertex { + inline int + operator()(const PointXYZRGBNormal& pt, CloudIHS& vertices, int& ind_o) const { - if (ind_o == -1) - { - ind_o = vertices.size (); - vertices.push_back (PointIHS (pt, -pt.normal_z)); - std::swap (vertices.back ().r, vertices.back ().b); + if (ind_o == -1) { + ind_o = vertices.size(); + vertices.push_back(PointIHS(pt, -pt.normal_z)); + std::swap(vertices.back().r, vertices.back().b); } return (ind_o); } @@ -517,44 +514,39 @@ pcl::ihs::OpenGLViewer::addMesh (const CloudXYZRGBNormalConstPtr& cloud, const s int ind_o_0, ind_o_1, ind_o_2, ind_o_3; // Index into the organized cloud. int ind_v_0, ind_v_1, ind_v_2, ind_v_3; // Index to the new vertices. - for (int r=1; roperator [] (ind_o_0); - const PointXYZRGBNormal& pt_1 = cloud->operator [] (ind_o_1); - const PointXYZRGBNormal& pt_2 = cloud->operator [] (ind_o_2); - const PointXYZRGBNormal& pt_3 = cloud->operator [] (ind_o_3); + const PointXYZRGBNormal& pt_0 = cloud->operator[](ind_o_0); + const PointXYZRGBNormal& pt_1 = cloud->operator[](ind_o_1); + const PointXYZRGBNormal& pt_2 = cloud->operator[](ind_o_2); + const PointXYZRGBNormal& pt_3 = cloud->operator[](ind_o_3); - if (!std::isnan (pt_1.x) && !std::isnan (pt_3.x)) - { - if (!std::isnan (pt_2.x)) // 1-2-3 is valid + if (!std::isnan(pt_1.x) && !std::isnan(pt_3.x)) { + if (!std::isnan(pt_2.x)) // 1-2-3 is valid { - if (std::abs (pt_1.z - pt_2.z) < 1 && - std::abs (pt_1.z - pt_3.z) < 1 && - std::abs (pt_2.z - pt_3.z) < 1) // distance threshold + if (std::abs(pt_1.z - pt_2.z) < 1 && std::abs(pt_1.z - pt_3.z) < 1 && + std::abs(pt_2.z - pt_3.z) < 1) // distance threshold { - ind_v_1 = addVertex (pt_1, vertices, indices [ind_o_1]); - ind_v_2 = addVertex (pt_2, vertices, indices [ind_o_2]); - ind_v_3 = addVertex (pt_3, vertices, indices [ind_o_3]); + ind_v_1 = addVertex(pt_1, vertices, indices[ind_o_1]); + ind_v_2 = addVertex(pt_2, vertices, indices[ind_o_2]); + ind_v_3 = addVertex(pt_3, vertices, indices[ind_o_3]); triangles.emplace_back(ind_v_1, ind_v_2, ind_v_3); } } - if (!std::isnan (pt_0.x)) // 0-1-3 is valid + if (!std::isnan(pt_0.x)) // 0-1-3 is valid { - if (std::abs (pt_0.z - pt_1.z) < 1 && - std::abs (pt_0.z - pt_3.z) < 1 && - std::abs (pt_1.z - pt_3.z) < 1) // distance threshold + if (std::abs(pt_0.z - pt_1.z) < 1 && std::abs(pt_0.z - pt_3.z) < 1 && + std::abs(pt_1.z - pt_3.z) < 1) // distance threshold { - ind_v_1 = addVertex (pt_1, vertices, indices [ind_o_1]); - ind_v_3 = addVertex (pt_3, vertices, indices [ind_o_3]); - ind_v_0 = addVertex (pt_0, vertices, indices [ind_o_0]); + ind_v_1 = addVertex(pt_1, vertices, indices[ind_o_1]); + ind_v_3 = addVertex(pt_3, vertices, indices[ind_o_3]); + ind_v_0 = addVertex(pt_0, vertices, indices[ind_o_0]); triangles.emplace_back(ind_v_1, ind_v_3, ind_v_0); } @@ -564,12 +556,12 @@ pcl::ihs::OpenGLViewer::addMesh (const CloudXYZRGBNormalConstPtr& cloud, const s } // Finally add the mesh. - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); - if (this->getMeshIsAdded (id)) - drawn_meshes_ [id] = mesh; + if (this->getMeshIsAdded(id)) + drawn_meshes_[id] = mesh; else - drawn_meshes_.insert (std::make_pair (id, mesh)); + drawn_meshes_.insert(std::make_pair(id, mesh)); return (true); } @@ -577,12 +569,13 @@ pcl::ihs::OpenGLViewer::addMesh (const CloudXYZRGBNormalConstPtr& cloud, const s //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OpenGLViewer::removeMesh (const std::string& id) +pcl::ihs::OpenGLViewer::removeMesh(const std::string& id) { - std::lock_guard lock (mutex_vis_); - if (!this->getMeshIsAdded (id)) return (false); + std::lock_guard lock(mutex_vis_); + if (!this->getMeshIsAdded(id)) + return (false); - drawn_meshes_.erase (id); + drawn_meshes_.erase(id); return (true); } @@ -590,34 +583,34 @@ pcl::ihs::OpenGLViewer::removeMesh (const std::string& id) //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::removeAllMeshes () +pcl::ihs::OpenGLViewer::removeAllMeshes() { - std::lock_guard lock (mutex_vis_); - drawn_meshes_.clear (); + std::lock_guard lock(mutex_vis_); + drawn_meshes_.clear(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setBoxCoefficients (const BoxCoefficients& coeffs) +pcl::ihs::OpenGLViewer::setBoxCoefficients(const BoxCoefficients& coeffs) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); box_coefficients_ = coeffs; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setDrawBox (const bool enabled) +pcl::ihs::OpenGLViewer::setDrawBox(const bool enabled) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); draw_box_ = enabled; } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OpenGLViewer::getDrawBox () const +pcl::ihs::OpenGLViewer::getDrawBox() const { return (draw_box_); } @@ -625,39 +618,38 @@ pcl::ihs::OpenGLViewer::getDrawBox () const //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setPivot (const Eigen::Vector3d& pivot) +pcl::ihs::OpenGLViewer::setPivot(const Eigen::Vector3d& pivot) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); cam_pivot_ = pivot; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setPivot (const std::string& id) +pcl::ihs::OpenGLViewer::setPivot(const std::string& id) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); cam_pivot_id_ = id; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::stopTimer () +pcl::ihs::OpenGLViewer::stopTimer() { - std::lock_guard lock (mutex_vis_); - if (timer_vis_) - { - timer_vis_->stop (); + std::lock_guard lock(mutex_vis_); + if (timer_vis_) { + timer_vis_->stop(); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setVisibilityConfidenceNormalization (const float vis_conf_norm) +pcl::ihs::OpenGLViewer::setVisibilityConfidenceNormalization(const float vis_conf_norm) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); vis_conf_norm_ = vis_conf_norm < 1 ? 1 : vis_conf_norm; } @@ -665,74 +657,88 @@ pcl::ihs::OpenGLViewer::setVisibilityConfidenceNormalization (const float vis_co //////////////////////////////////////////////////////////////////////////////// QSize -pcl::ihs::OpenGLViewer::minimumSizeHint () const +pcl::ihs::OpenGLViewer::minimumSizeHint() const { - return (QSize (160, 120)); + return (QSize(160, 120)); } //////////////////////////////////////////////////////////////////////////////// QSize -pcl::ihs::OpenGLViewer::sizeHint () const +pcl::ihs::OpenGLViewer::sizeHint() const { - return (QSize (640, 480)); + return (QSize(640, 480)); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setScalingFactor (const double scale) +pcl::ihs::OpenGLViewer::setScalingFactor(const double scale) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); scaling_factor_ = scale; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::timerCallback () +pcl::ihs::OpenGLViewer::timerCallback() { - QWidget::update (); + QWidget::update(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::resetCamera () +pcl::ihs::OpenGLViewer::resetCamera() { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); - R_cam_ = Eigen::Quaterniond (1., 0., 0., 0.); - t_cam_ = Eigen::Vector3d (0., 0., 0.); + R_cam_ = Eigen::Quaterniond(1., 0., 0., 0.); + t_cam_ = Eigen::Vector3d(0., 0., 0.); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::toggleMeshRepresentation () +pcl::ihs::OpenGLViewer::toggleMeshRepresentation() { - switch (mesh_representation_) - { - case MR_POINTS: this->setMeshRepresentation (MR_EDGES); break; - case MR_EDGES: this->setMeshRepresentation (MR_FACES); break; - case MR_FACES: this->setMeshRepresentation (MR_POINTS); break; - default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; exit (EXIT_FAILURE); + switch (mesh_representation_) { + case MR_POINTS: + this->setMeshRepresentation(MR_EDGES); + break; + case MR_EDGES: + this->setMeshRepresentation(MR_FACES); + break; + case MR_FACES: + this->setMeshRepresentation(MR_POINTS); + break; + default: + std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; + exit(EXIT_FAILURE); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setMeshRepresentation (const MeshRepresentation& representation) +pcl::ihs::OpenGLViewer::setMeshRepresentation(const MeshRepresentation& representation) { - std::lock_guard lock (mutex_vis_); - - switch (mesh_representation_) - { - case MR_POINTS: std::cerr << "Drawing the points.\n"; break; - case MR_EDGES: std::cerr << "Drawing the edges.\n"; break; - case MR_FACES: std::cerr << "Drawing the faces.\n"; break; - default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; exit (EXIT_FAILURE); + std::lock_guard lock(mutex_vis_); + + switch (mesh_representation_) { + case MR_POINTS: + std::cerr << "Drawing the points.\n"; + break; + case MR_EDGES: + std::cerr << "Drawing the edges.\n"; + break; + case MR_FACES: + std::cerr << "Drawing the faces.\n"; + break; + default: + std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; + exit(EXIT_FAILURE); } mesh_representation_ = representation; @@ -741,30 +747,44 @@ pcl::ihs::OpenGLViewer::setMeshRepresentation (const MeshRepresentation& represe //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::toggleColoring () +pcl::ihs::OpenGLViewer::toggleColoring() { - switch (coloring_) - { - case COL_RGB: this->setColoring (COL_ONE_COLOR); break; - case COL_ONE_COLOR: this->setColoring (COL_VISCONF); break; - case COL_VISCONF: this->setColoring (COL_RGB); break; - default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; exit (EXIT_FAILURE); + switch (coloring_) { + case COL_RGB: + this->setColoring(COL_ONE_COLOR); + break; + case COL_ONE_COLOR: + this->setColoring(COL_VISCONF); + break; + case COL_VISCONF: + this->setColoring(COL_RGB); + break; + default: + std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; + exit(EXIT_FAILURE); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setColoring (const Coloring& coloring) +pcl::ihs::OpenGLViewer::setColoring(const Coloring& coloring) { - std::lock_guard lock (mutex_vis_); - - switch (coloring) - { - case COL_RGB: std::cerr << "Coloring according to the rgb values.\n"; break; - case COL_ONE_COLOR: std::cerr << "Use one color for all points.\n"; break; - case COL_VISCONF: std::cerr << "Coloring according to the visibility confidence.\n"; break; - default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; exit (EXIT_FAILURE); + std::lock_guard lock(mutex_vis_); + + switch (coloring) { + case COL_RGB: + std::cerr << "Coloring according to the rgb values.\n"; + break; + case COL_ONE_COLOR: + std::cerr << "Use one color for all points.\n"; + break; + case COL_VISCONF: + std::cerr << "Coloring according to the visibility confidence.\n"; + break; + default: + std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; + exit(EXIT_FAILURE); } coloring_ = coloring; } @@ -772,16 +792,16 @@ pcl::ihs::OpenGLViewer::setColoring (const Coloring& coloring) //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::paintEvent (QPaintEvent* /*event*/) +pcl::ihs::OpenGLViewer::paintEvent(QPaintEvent* /*event*/) { - this->calcPivot (); - this->makeCurrent (); + this->calcPivot(); + this->makeCurrent(); // Clear information from the last draw - glClearColor (0.f, 0.f, 0.f, 0.f); - glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glClearColor(0.f, 0.f, 0.f, 0.f); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - this->setupViewport (this->width (), this->height ()); + this->setupViewport(this->width(), this->height()); // Move light with camera (see example 5-7) // http://www.glprogramming.com/red/chapter05.html @@ -789,258 +809,281 @@ pcl::ihs::OpenGLViewer::paintEvent (QPaintEvent* /*event*/) glLoadIdentity(); // light 0 (directional) - glLightfv (GL_LIGHT0, GL_AMBIENT , Eigen::Vector4f (0.1f, 0.1f, 0.1f, 1.0f).eval ().data ()); - glLightfv (GL_LIGHT0, GL_DIFFUSE , Eigen::Vector4f (0.6f, 0.6f, 0.6f, 1.0f).eval ().data () ); - glLightfv (GL_LIGHT0, GL_SPECULAR, Eigen::Vector4f (0.2f, 0.2f, 0.2f, 1.0f).eval ().data ()); - glLightfv (GL_LIGHT0, GL_POSITION, Eigen::Vector4f (0.3f, 0.5f, 0.8f, 0.0f).normalized ().eval ().data ()); + glLightfv( + GL_LIGHT0, GL_AMBIENT, Eigen::Vector4f(0.1f, 0.1f, 0.1f, 1.0f).eval().data()); + glLightfv( + GL_LIGHT0, GL_DIFFUSE, Eigen::Vector4f(0.6f, 0.6f, 0.6f, 1.0f).eval().data()); + glLightfv( + GL_LIGHT0, GL_SPECULAR, Eigen::Vector4f(0.2f, 0.2f, 0.2f, 1.0f).eval().data()); + glLightfv(GL_LIGHT0, + GL_POSITION, + Eigen::Vector4f(0.3f, 0.5f, 0.8f, 0.0f).normalized().eval().data()); // light 1 (directional) - glLightfv (GL_LIGHT1, GL_AMBIENT , Eigen::Vector4f ( 0.0f, 0.0f, 0.0f, 1.0f).eval ().data ()); - glLightfv (GL_LIGHT1, GL_DIFFUSE , Eigen::Vector4f ( 0.3f, 0.3f, 0.3f, 1.0f).eval ().data () ); - glLightfv (GL_LIGHT1, GL_SPECULAR, Eigen::Vector4f ( 0.1f, 0.1f, 0.1f, 1.0f).eval ().data ()); - glLightfv (GL_LIGHT1, GL_POSITION, Eigen::Vector4f (-0.3f, 0.5f, 0.8f, 0.0f).normalized ().eval ().data ()); + glLightfv( + GL_LIGHT1, GL_AMBIENT, Eigen::Vector4f(0.0f, 0.0f, 0.0f, 1.0f).eval().data()); + glLightfv( + GL_LIGHT1, GL_DIFFUSE, Eigen::Vector4f(0.3f, 0.3f, 0.3f, 1.0f).eval().data()); + glLightfv( + GL_LIGHT1, GL_SPECULAR, Eigen::Vector4f(0.1f, 0.1f, 0.1f, 1.0f).eval().data()); + glLightfv(GL_LIGHT1, + GL_POSITION, + Eigen::Vector4f(-0.3f, 0.5f, 0.8f, 0.0f).normalized().eval().data()); // Material - glColorMaterial (GL_FRONT, GL_AMBIENT_AND_DIFFUSE); - glMaterialfv (GL_FRONT, GL_SPECULAR , Eigen::Vector4f (0.1f, 0.1f, 0.1f, 1.0f).eval ().data ()); - glMaterialf (GL_FRONT, GL_SHININESS, 25.f); - - glEnable (GL_DEPTH_TEST); - glEnable (GL_NORMALIZE); - glEnable (GL_COLOR_MATERIAL); - glEnable (GL_LIGHTING); - glEnable (GL_LIGHT0); - glEnable (GL_LIGHT1); + glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE); + glMaterialfv( + GL_FRONT, GL_SPECULAR, Eigen::Vector4f(0.1f, 0.1f, 0.1f, 1.0f).eval().data()); + glMaterialf(GL_FRONT, GL_SHININESS, 25.f); + + glEnable(GL_DEPTH_TEST); + glEnable(GL_NORMALIZE); + glEnable(GL_COLOR_MATERIAL); + glEnable(GL_LIGHTING); + glEnable(GL_LIGHT0); + glEnable(GL_LIGHT1); // Projection matrix - glMatrixMode (GL_PROJECTION); - glLoadIdentity (); - gluPerspective (43., 4./3., 0.01 / scaling_factor_, 10. / scaling_factor_); - glMatrixMode (GL_MODELVIEW); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + gluPerspective(43., 4. / 3., 0.01 / scaling_factor_, 10. / scaling_factor_); + glMatrixMode(GL_MODELVIEW); // ModelView matrix Eigen::Quaterniond R_cam; - Eigen::Vector3d t_cam; + Eigen::Vector3d t_cam; { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); R_cam = R_cam_; t_cam = t_cam_; } - const Eigen::Vector3d o = Eigen::Vector3d::Zero (); - const Eigen::Vector3d ey = Eigen::Vector3d::UnitY (); - const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ (); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Vector3d ey = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ(); - const Eigen::Vector3d eye = R_cam * o + t_cam; - const Eigen::Vector3d center = R_cam * ez + t_cam; - const Eigen::Vector3d up = (R_cam * -ey).normalized (); + const Eigen::Vector3d eye = R_cam * o + t_cam; + const Eigen::Vector3d center = R_cam * ez + t_cam; + const Eigen::Vector3d up = (R_cam * -ey).normalized(); - glMatrixMode (GL_MODELVIEW); - gluLookAt (eye. x (), eye. y (), eye. z (), - center.x (), center.y (), center.z (), - up. x (), up. y (), up. z ()); + glMatrixMode(GL_MODELVIEW); + gluLookAt(eye.x(), + eye.y(), + eye.z(), + center.x(), + center.y(), + center.z(), + up.x(), + up.y(), + up.z()); // Draw everything - this->drawMeshes (); + this->drawMeshes(); - glDisable (GL_LIGHTING); // This is needed so the color is right. - this->drawBox (); + glDisable(GL_LIGHTING); // This is needed so the color is right. + this->drawBox(); } //////////////////////////////////////////////////////////////////////////////// bool -pcl::ihs::OpenGLViewer::getMeshIsAdded (const std::string& id) +pcl::ihs::OpenGLViewer::getMeshIsAdded(const std::string& id) { // std::lock_guard lock (mutex_vis_); - return (drawn_meshes_.find (id) != drawn_meshes_.end ()); + return (drawn_meshes_.find(id) != drawn_meshes_.end()); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::calcPivot () +pcl::ihs::OpenGLViewer::calcPivot() { - std::unique_lock lock (mutex_vis_); - if (this->getMeshIsAdded (cam_pivot_id_)) - { + std::unique_lock lock(mutex_vis_); + if (this->getMeshIsAdded(cam_pivot_id_)) { Eigen::Vector4f pivot; - const FaceVertexMeshConstPtr mesh = drawn_meshes_ [cam_pivot_id_]; + const FaceVertexMeshConstPtr mesh = drawn_meshes_[cam_pivot_id_]; - if (pcl::compute3DCentroid (mesh->vertices, pivot)) - { - const Eigen::Vector3d p = mesh->transformation * pivot.head <3> ().cast (); - lock.unlock (); - this->setPivot (p); + if (pcl::compute3DCentroid(mesh->vertices, pivot)) { + const Eigen::Vector3d p = mesh->transformation * pivot.head<3>().cast(); + lock.unlock(); + this->setPivot(p); } } - cam_pivot_id_.clear (); + cam_pivot_id_.clear(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::drawMeshes () +pcl::ihs::OpenGLViewer::drawMeshes() { - std::lock_guard lock (mutex_vis_); - - glEnableClientState (GL_VERTEX_ARRAY); - glEnableClientState (GL_NORMAL_ARRAY); - switch (coloring_) - { - case COL_RGB: glEnableClientState (GL_COLOR_ARRAY); break; - case COL_ONE_COLOR: glDisableClientState (GL_COLOR_ARRAY); break; - case COL_VISCONF: glEnableClientState (GL_COLOR_ARRAY); break; - default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; exit (EXIT_FAILURE); + std::lock_guard lock(mutex_vis_); + + glEnableClientState(GL_VERTEX_ARRAY); + glEnableClientState(GL_NORMAL_ARRAY); + switch (coloring_) { + case COL_RGB: + glEnableClientState(GL_COLOR_ARRAY); + break; + case COL_ONE_COLOR: + glDisableClientState(GL_COLOR_ARRAY); + break; + case COL_VISCONF: + glEnableClientState(GL_COLOR_ARRAY); + break; + default: + std::cerr << "ERROR in opengl_viewer.cpp: Unknown coloring\n"; + exit(EXIT_FAILURE); } - switch (mesh_representation_) - { - case MR_POINTS: break; - case MR_EDGES: glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); break; - case MR_FACES: glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); break; - default: std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; exit (EXIT_FAILURE); + switch (mesh_representation_) { + case MR_POINTS: + break; + case MR_EDGES: + glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); + break; + case MR_FACES: + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + break; + default: + std::cerr << "ERROR in opengl_viewer.cpp: Unknown mesh representation\n"; + exit(EXIT_FAILURE); } - for (FaceVertexMeshMap::const_iterator it=drawn_meshes_.begin (); it!=drawn_meshes_.end (); ++it) - { - if (it->second && !it->second->vertices.empty ()) - { - const FaceVertexMesh& mesh = *it->second; + for (const auto& drawn_mesh : drawn_meshes_) { + if (drawn_mesh.second && !drawn_mesh.second->vertices.empty()) { + const FaceVertexMesh& mesh = *drawn_mesh.second; - glVertexPointer (3, GL_FLOAT, sizeof (PointIHS), &(mesh.vertices [0].x )); - glNormalPointer ( GL_FLOAT, sizeof (PointIHS), &(mesh.vertices [0].normal_x)); + glVertexPointer(3, GL_FLOAT, sizeof(PointIHS), &(mesh.vertices[0].x)); + glNormalPointer(GL_FLOAT, sizeof(PointIHS), &(mesh.vertices[0].normal_x)); - Colors colors (3, mesh.vertices.size ()); + Colors colors(3, mesh.vertices.size()); - switch (coloring_) - { - case COL_RGB: - { - glColorPointer (3, GL_UNSIGNED_BYTE, sizeof (PointIHS), &(mesh.vertices [0].b)); - break; - } - case COL_ONE_COLOR: - { - glColor3f (.7f, .7f, .7f); - break; + switch (coloring_) { + case COL_RGB: { + glColorPointer(3, GL_UNSIGNED_BYTE, sizeof(PointIHS), &(mesh.vertices[0].b)); + break; + } + case COL_ONE_COLOR: { + glColor3f(.7f, .7f, .7f); + break; + } + case COL_VISCONF: { + for (std::size_t i = 0; i < mesh.vertices.size(); ++i) { + const unsigned int n = pcl::ihs::countDirections(mesh.vertices[i].directions); + const unsigned int index = + static_cast(static_cast(colormap_.cols()) * + static_cast(n) / vis_conf_norm_); + + colors.col(i) = colormap_.col(index < 256 ? index : 255); } - case COL_VISCONF: - { - for (std::size_t i=0; i ( - static_cast (colormap_.cols ()) * - static_cast (n) / vis_conf_norm_); - - colors.col (i) = colormap_.col (index < 256 ? index : 255); - } - glColorPointer (3, GL_UNSIGNED_BYTE, 0, colors.data ()); - } + glColorPointer(3, GL_UNSIGNED_BYTE, 0, colors.data()); + } } - glPushMatrix (); + glPushMatrix(); { - glMultMatrixd (mesh.transformation.matrix ().data ()); + glMultMatrixd(mesh.transformation.matrix().data()); - switch (mesh_representation_) - { - case MR_POINTS: - { - glDrawArrays (GL_POINTS, 0, mesh.vertices.size ()); - break; - } - case MR_EDGES: case MR_FACES: - { - glDrawElements (GL_TRIANGLES, 3*mesh.triangles.size (), GL_UNSIGNED_INT, &mesh.triangles [0]); - break; - } + switch (mesh_representation_) { + case MR_POINTS: { + glDrawArrays(GL_POINTS, 0, mesh.vertices.size()); + break; + } + case MR_EDGES: + case MR_FACES: { + glDrawElements(GL_TRIANGLES, + 3 * mesh.triangles.size(), + GL_UNSIGNED_INT, + &mesh.triangles[0]); + break; + } } } - glPopMatrix (); + glPopMatrix(); } } - glDisableClientState (GL_VERTEX_ARRAY); - glDisableClientState (GL_NORMAL_ARRAY); - glDisableClientState (GL_COLOR_ARRAY); - glPolygonMode (GL_FRONT_AND_BACK, GL_FILL); + glDisableClientState(GL_VERTEX_ARRAY); + glDisableClientState(GL_NORMAL_ARRAY); + glDisableClientState(GL_COLOR_ARRAY); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::drawBox () +pcl::ihs::OpenGLViewer::drawBox() { BoxCoefficients coeffs; { - std::lock_guard lock (mutex_vis_); - if (draw_box_) coeffs = box_coefficients_; - else return; + std::lock_guard lock(mutex_vis_); + if (draw_box_) + coeffs = box_coefficients_; + else + return; } - glColor3f (1.f, 1.f, 1.f); + glColor3f(1.f, 1.f, 1.f); - glPushMatrix (); + glPushMatrix(); { - glMultMatrixd (coeffs.transformation.matrix ().data ()); + glMultMatrixd(coeffs.transformation.matrix().data()); // Front glBegin(GL_LINE_STRIP); { - glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_min); - glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_min); - glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_min); - glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_min); - glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_min); + glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_min); + glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_min); + glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_min); + glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_min); + glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_min); } glEnd(); // Back - glBegin (GL_LINE_STRIP); + glBegin(GL_LINE_STRIP); { - glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_max); - glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_max); - glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_max); - glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_max); - glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_max); + glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_max); + glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_max); + glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_max); + glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_max); + glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_max); } glEnd(); // Sides - glBegin (GL_LINES); + glBegin(GL_LINES); { - glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_min); - glVertex3f (coeffs.x_min, coeffs.y_min, coeffs.z_max); + glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_min); + glVertex3f(coeffs.x_min, coeffs.y_min, coeffs.z_max); - glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_min); - glVertex3f (coeffs.x_max, coeffs.y_min, coeffs.z_max); + glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_min); + glVertex3f(coeffs.x_max, coeffs.y_min, coeffs.z_max); - glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_min); - glVertex3f (coeffs.x_max, coeffs.y_max, coeffs.z_max); + glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_min); + glVertex3f(coeffs.x_max, coeffs.y_max, coeffs.z_max); - glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_min); - glVertex3f (coeffs.x_min, coeffs.y_max, coeffs.z_max); + glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_min); + glVertex3f(coeffs.x_min, coeffs.y_max, coeffs.z_max); } glEnd(); } - glPopMatrix (); + glPopMatrix(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::initializeGL () -{ -} +pcl::ihs::OpenGLViewer::initializeGL() +{} //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::setupViewport (const int w, const int h) +pcl::ihs::OpenGLViewer::setupViewport(const int w, const int h) { - const float aspect_ratio = 4./3.; + const float aspect_ratio = 4. / 3.; // Use the biggest possible area of the window to draw to // case 1 (w < w_scaled): case 2 (w >= w_scaled): @@ -1053,105 +1096,107 @@ pcl::ihs::OpenGLViewer::setupViewport (const int w, const int h) const float w_scaled = h * aspect_ratio; const float h_scaled = w / aspect_ratio; - if (w < w_scaled) - { - glViewport (0, static_cast ((h - h_scaled) / 2.f), w, static_cast (h_scaled)); + if (w < w_scaled) { + glViewport( + 0, static_cast((h - h_scaled) / 2.f), w, static_cast(h_scaled)); } - else - { - glViewport (static_cast ((w - w_scaled) / 2.f), 0, static_cast (w_scaled), h); + else { + glViewport( + static_cast((w - w_scaled) / 2.f), 0, static_cast(w_scaled), h); } } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::resizeGL (int w, int h) +pcl::ihs::OpenGLViewer::resizeGL(int w, int h) { - this->setupViewport (w, h); + this->setupViewport(w, h); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::mousePressEvent (QMouseEvent* /*event*/) +pcl::ihs::OpenGLViewer::mousePressEvent(QMouseEvent* /*event*/) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); mouse_pressed_begin_ = true; } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::mouseMoveEvent (QMouseEvent* event) +pcl::ihs::OpenGLViewer::mouseMoveEvent(QMouseEvent* event) { - std::lock_guard lock (mutex_vis_); + std::lock_guard lock(mutex_vis_); - if (mouse_pressed_begin_) - { - x_prev_ = event->pos ().x (); - y_prev_ = event->pos ().y (); + if (mouse_pressed_begin_) { + x_prev_ = event->pos().x(); + y_prev_ = event->pos().y(); mouse_pressed_begin_ = false; return; } - if (event->pos ().x () == x_prev_ && event->pos ().y () == y_prev_) return; - if (this->width () == 0 || this->height () == 0) return; + if (event->pos().x() == x_prev_ && event->pos().y() == y_prev_) + return; + if (this->width() == 0 || this->height() == 0) + return; - const double dx = static_cast (event->pos ().x ()) - static_cast (x_prev_); - const double dy = static_cast (event->pos ().y ()) - static_cast (y_prev_); - const double w = static_cast (this->width ()); - const double h = static_cast (this->height ()); - const double d = std::sqrt (w*w + h*h); + const double dx = + static_cast(event->pos().x()) - static_cast(x_prev_); + const double dy = + static_cast(event->pos().y()) - static_cast(y_prev_); + const double w = static_cast(this->width()); + const double h = static_cast(this->height()); + const double d = std::sqrt(w * w + h * h); - const Eigen::Vector3d o = Eigen::Vector3d::Zero (); - const Eigen::Vector3d ex = Eigen::Vector3d::UnitX (); - const Eigen::Vector3d ey = Eigen::Vector3d::UnitY (); - const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ (); + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Vector3d ex = Eigen::Vector3d::UnitX(); + const Eigen::Vector3d ey = Eigen::Vector3d::UnitY(); + const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ(); // Scale with the distance between the pivot and camera eye. - const double scale = std::max ((cam_pivot_ - R_cam_ * o - t_cam_).norm (), .1 / scaling_factor_) / d; + const double scale = + std::max((cam_pivot_ - R_cam_ * o - t_cam_).norm(), .1 / scaling_factor_) / d; - if (QApplication::mouseButtons () == Qt::LeftButton) - { - const double rot_angle = -7. * std::atan (std::sqrt ((dx*dx + dy*dy)) / d); - const Eigen::Vector3d rot_axis = (R_cam_ * ex * dy - R_cam_ * ey * dx).normalized (); + if (QApplication::mouseButtons() == Qt::LeftButton) { + const double rot_angle = -7. * std::atan(std::sqrt((dx * dx + dy * dy)) / d); + const Eigen::Vector3d rot_axis = (R_cam_ * ex * dy - R_cam_ * ey * dx).normalized(); - const Eigen::Quaterniond dR (Eigen::AngleAxisd (rot_angle, rot_axis)); + const Eigen::Quaterniond dR(Eigen::AngleAxisd(rot_angle, rot_axis)); t_cam_ = dR * (t_cam_ - cam_pivot_) + cam_pivot_; - R_cam_ = (dR * R_cam_).normalized (); + R_cam_ = (dR * R_cam_).normalized(); } - else if (QApplication::mouseButtons () == Qt::MiddleButton) - { - t_cam_ += 1.3 * scale * Eigen::Vector3d (R_cam_ * (ey * -dy + ex * -dx)); + else if (QApplication::mouseButtons() == Qt::MiddleButton) { + t_cam_ += 1.3 * scale * Eigen::Vector3d(R_cam_ * (ey * -dy + ex * -dx)); } - else if (QApplication::mouseButtons () == Qt::RightButton) - { - t_cam_ += 2.6 * scale * Eigen::Vector3d (R_cam_ * (ez * -dy)); + else if (QApplication::mouseButtons() == Qt::RightButton) { + t_cam_ += 2.6 * scale * Eigen::Vector3d(R_cam_ * (ez * -dy)); } - x_prev_ = event->pos ().x (); - y_prev_ = event->pos ().y (); + x_prev_ = event->pos().x(); + y_prev_ = event->pos().y(); } //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::OpenGLViewer::wheelEvent (QWheelEvent* event) +pcl::ihs::OpenGLViewer::wheelEvent(QWheelEvent* event) { - if (QApplication::mouseButtons () == Qt::NoButton) - { - std::lock_guard lock (mutex_vis_); + if (QApplication::mouseButtons() == Qt::NoButton) { + std::lock_guard lock(mutex_vis_); // Scale with the distance between the pivot and camera eye. - const Eigen::Vector3d o = Eigen::Vector3d::Zero (); - const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ (); - const double w = static_cast (this->width ()); - const double h = static_cast (this->height ()); - const double d = std::sqrt (w*w + h*h); - const double scale = std::max ((cam_pivot_ - R_cam_ * o - t_cam_).norm (), .1 / scaling_factor_) / d; + const Eigen::Vector3d o = Eigen::Vector3d::Zero(); + const Eigen::Vector3d ez = Eigen::Vector3d::UnitZ(); + const double w = static_cast(this->width()); + const double h = static_cast(this->height()); + const double d = std::sqrt(w * w + h * h); + const double scale = + std::max((cam_pivot_ - R_cam_ * o - t_cam_).norm(), .1 / scaling_factor_) / d; // http://doc.qt.digia.com/qt/qwheelevent.html#delta - t_cam_ += scale * Eigen::Vector3d (R_cam_ * (ez * static_cast (event->delta ()))); + t_cam_ += + scale * Eigen::Vector3d(R_cam_ * (ez * static_cast(event->delta()))); } } diff --git a/apps/in_hand_scanner/src/visibility_confidence.cpp b/apps/in_hand_scanner/src/visibility_confidence.cpp index fb2ff56f..a067eff6 100644 --- a/apps/in_hand_scanner/src/visibility_confidence.cpp +++ b/apps/in_hand_scanner/src/visibility_confidence.cpp @@ -39,97 +39,95 @@ */ #include + #include // for Isometry3f -pcl::ihs::Dome::Dome () +pcl::ihs::Dome::Dome() { - vertices_.col ( 0) = Eigen::Vector4f (-0.000000119f, 0.000000000f, 1.000000000f, 0.f); - vertices_.col ( 1) = Eigen::Vector4f ( 0.894427180f, 0.000000000f, 0.447213739f, 0.f); - vertices_.col ( 2) = Eigen::Vector4f ( 0.276393145f, 0.850650907f, 0.447213650f, 0.f); - vertices_.col ( 3) = Eigen::Vector4f (-0.723606884f, 0.525731146f, 0.447213531f, 0.f); - vertices_.col ( 4) = Eigen::Vector4f (-0.723606884f, -0.525731146f, 0.447213531f, 0.f); - vertices_.col ( 5) = Eigen::Vector4f ( 0.276393145f, -0.850650907f, 0.447213650f, 0.f); - vertices_.col ( 6) = Eigen::Vector4f ( 0.343278527f, 0.000000000f, 0.939233720f, 0.f); - vertices_.col ( 7) = Eigen::Vector4f ( 0.686557174f, 0.000000000f, 0.727075875f, 0.f); - vertices_.col ( 8) = Eigen::Vector4f ( 0.792636156f, 0.326477438f, 0.514918089f, 0.f); - vertices_.col ( 9) = Eigen::Vector4f ( 0.555436373f, 0.652954817f, 0.514918029f, 0.f); - vertices_.col (10) = Eigen::Vector4f ( 0.106078848f, 0.326477438f, 0.939233720f, 0.f); - vertices_.col (11) = Eigen::Vector4f ( 0.212157741f, 0.652954817f, 0.727075756f, 0.f); - vertices_.col (12) = Eigen::Vector4f (-0.065560505f, 0.854728878f, 0.514917910f, 0.f); - vertices_.col (13) = Eigen::Vector4f (-0.449357629f, 0.730025530f, 0.514917850f, 0.f); - vertices_.col (14) = Eigen::Vector4f (-0.277718395f, 0.201774135f, 0.939233661f, 0.f); - vertices_.col (15) = Eigen::Vector4f (-0.555436671f, 0.403548241f, 0.727075696f, 0.f); - vertices_.col (16) = Eigen::Vector4f (-0.833154857f, 0.201774105f, 0.514917850f, 0.f); - vertices_.col (17) = Eigen::Vector4f (-0.833154857f, -0.201774150f, 0.514917850f, 0.f); - vertices_.col (18) = Eigen::Vector4f (-0.277718395f, -0.201774135f, 0.939233661f, 0.f); - vertices_.col (19) = Eigen::Vector4f (-0.555436671f, -0.403548241f, 0.727075696f, 0.f); - vertices_.col (20) = Eigen::Vector4f (-0.449357659f, -0.730025649f, 0.514917910f, 0.f); - vertices_.col (21) = Eigen::Vector4f (-0.065560460f, -0.854728937f, 0.514917850f, 0.f); - vertices_.col (22) = Eigen::Vector4f ( 0.106078848f, -0.326477438f, 0.939233720f, 0.f); - vertices_.col (23) = Eigen::Vector4f ( 0.212157741f, -0.652954817f, 0.727075756f, 0.f); - vertices_.col (24) = Eigen::Vector4f ( 0.555436373f, -0.652954757f, 0.514917970f, 0.f); - vertices_.col (25) = Eigen::Vector4f ( 0.792636156f, -0.326477349f, 0.514918089f, 0.f); - vertices_.col (26) = Eigen::Vector4f ( 0.491123378f, 0.356822133f, 0.794654608f, 0.f); - vertices_.col (27) = Eigen::Vector4f (-0.187592626f, 0.577350259f, 0.794654429f, 0.f); - vertices_.col (28) = Eigen::Vector4f (-0.607062101f, -0.000000016f, 0.794654369f, 0.f); - vertices_.col (29) = Eigen::Vector4f (-0.187592626f, -0.577350378f, 0.794654489f, 0.f); - vertices_.col (30) = Eigen::Vector4f ( 0.491123348f, -0.356822133f, 0.794654548f, 0.f); - - for (Eigen::Index i=0; i < vertices_.cols (); ++i) - { - vertices_.col (i).head <3> ().normalize (); + vertices_.col(0) = Eigen::Vector4f(-0.000000119f, 0.000000000f, 1.000000000f, 0.f); + vertices_.col(1) = Eigen::Vector4f(0.894427180f, 0.000000000f, 0.447213739f, 0.f); + vertices_.col(2) = Eigen::Vector4f(0.276393145f, 0.850650907f, 0.447213650f, 0.f); + vertices_.col(3) = Eigen::Vector4f(-0.723606884f, 0.525731146f, 0.447213531f, 0.f); + vertices_.col(4) = Eigen::Vector4f(-0.723606884f, -0.525731146f, 0.447213531f, 0.f); + vertices_.col(5) = Eigen::Vector4f(0.276393145f, -0.850650907f, 0.447213650f, 0.f); + vertices_.col(6) = Eigen::Vector4f(0.343278527f, 0.000000000f, 0.939233720f, 0.f); + vertices_.col(7) = Eigen::Vector4f(0.686557174f, 0.000000000f, 0.727075875f, 0.f); + vertices_.col(8) = Eigen::Vector4f(0.792636156f, 0.326477438f, 0.514918089f, 0.f); + vertices_.col(9) = Eigen::Vector4f(0.555436373f, 0.652954817f, 0.514918029f, 0.f); + vertices_.col(10) = Eigen::Vector4f(0.106078848f, 0.326477438f, 0.939233720f, 0.f); + vertices_.col(11) = Eigen::Vector4f(0.212157741f, 0.652954817f, 0.727075756f, 0.f); + vertices_.col(12) = Eigen::Vector4f(-0.065560505f, 0.854728878f, 0.514917910f, 0.f); + vertices_.col(13) = Eigen::Vector4f(-0.449357629f, 0.730025530f, 0.514917850f, 0.f); + vertices_.col(14) = Eigen::Vector4f(-0.277718395f, 0.201774135f, 0.939233661f, 0.f); + vertices_.col(15) = Eigen::Vector4f(-0.555436671f, 0.403548241f, 0.727075696f, 0.f); + vertices_.col(16) = Eigen::Vector4f(-0.833154857f, 0.201774105f, 0.514917850f, 0.f); + vertices_.col(17) = Eigen::Vector4f(-0.833154857f, -0.201774150f, 0.514917850f, 0.f); + vertices_.col(18) = Eigen::Vector4f(-0.277718395f, -0.201774135f, 0.939233661f, 0.f); + vertices_.col(19) = Eigen::Vector4f(-0.555436671f, -0.403548241f, 0.727075696f, 0.f); + vertices_.col(20) = Eigen::Vector4f(-0.449357659f, -0.730025649f, 0.514917910f, 0.f); + vertices_.col(21) = Eigen::Vector4f(-0.065560460f, -0.854728937f, 0.514917850f, 0.f); + vertices_.col(22) = Eigen::Vector4f(0.106078848f, -0.326477438f, 0.939233720f, 0.f); + vertices_.col(23) = Eigen::Vector4f(0.212157741f, -0.652954817f, 0.727075756f, 0.f); + vertices_.col(24) = Eigen::Vector4f(0.555436373f, -0.652954757f, 0.514917970f, 0.f); + vertices_.col(25) = Eigen::Vector4f(0.792636156f, -0.326477349f, 0.514918089f, 0.f); + vertices_.col(26) = Eigen::Vector4f(0.491123378f, 0.356822133f, 0.794654608f, 0.f); + vertices_.col(27) = Eigen::Vector4f(-0.187592626f, 0.577350259f, 0.794654429f, 0.f); + vertices_.col(28) = Eigen::Vector4f(-0.607062101f, -0.000000016f, 0.794654369f, 0.f); + vertices_.col(29) = Eigen::Vector4f(-0.187592626f, -0.577350378f, 0.794654489f, 0.f); + vertices_.col(30) = Eigen::Vector4f(0.491123348f, -0.356822133f, 0.794654548f, 0.f); + + for (Eigen::Index i = 0; i < vertices_.cols(); ++i) { + vertices_.col(i).head<3>().normalize(); } } //////////////////////////////////////////////////////////////////////////////// pcl::ihs::Dome::Vertices -pcl::ihs::Dome::getVertices () const +pcl::ihs::Dome::getVertices() const { return (vertices_); } //////////////////////////////////////////////////////////////////////////////// -namespace pcl -{ - namespace ihs - { - static const pcl::ihs::Dome dome; - } // End namespace ihs +namespace pcl { +namespace ihs { +static const pcl::ihs::Dome dome; +} // End namespace ihs } // End namespace pcl //////////////////////////////////////////////////////////////////////////////// void -pcl::ihs::addDirection (const Eigen::Vector4f& normal, - const Eigen::Vector4f& direction, - std::uint32_t& directions) +pcl::ihs::addDirection(const Eigen::Vector4f& normal, + const Eigen::Vector4f& direction, + std::uint32_t& directions) { // Find the rotation that aligns the normal with [0; 0; 1] - const float dot = normal.z (); + const float dot = normal.z(); - Eigen::Isometry3f R = Eigen::Isometry3f::Identity (); + Eigen::Isometry3f R = Eigen::Isometry3f::Identity(); - // No need to transform if the normal is already very close to [0; 0; 1] (also avoids numerical issues) + // No need to transform if the normal is already very close to [0; 0; 1] (also avoids + // numerical issues) // TODO: The threshold is hard coded for a frequency=3. // It can be calculated with // - max_z = maximum z value of the dome vertices (excluding [0; 0; 1]) // - thresh = std::cos (std::acos (max_z) / 2) // - always round up! // - with max_z = 0.939 -> thresh = 0.9847 ~ 0.985 - if (dot <= .985f) - { - const Eigen::Vector3f axis = Eigen::Vector3f (normal.y (), -normal.x (), 0.f).normalized (); - R = Eigen::Isometry3f (Eigen::AngleAxisf (std::acos (dot), axis)); + if (dot <= .985f) { + const auto axis = Eigen::Vector3f(normal.y(), -normal.x(), 0.f).normalized(); + R = Eigen::Isometry3f(Eigen::AngleAxisf(std::acos(dot), axis)); } - // Transform the direction into the dome coordinate system (which is aligned with the normal) + // Transform the direction into the dome coordinate system (which is aligned with the + // normal) Eigen::Vector4f aligned_direction = (R * direction); - aligned_direction.head <3> ().normalize (); + aligned_direction.head<3>().normalize(); - if (aligned_direction.z () < 0) - { + if (aligned_direction.z() < 0) { return; } @@ -138,7 +136,9 @@ pcl::ihs::addDirection (const Eigen::Vector4f& normal, // std::acos (angle) = dot (a, b) / (norm (a) * norm (b) // m_sphere_vertices are already normalized unsigned int index = 0; - aligned_direction.transpose ().lazyProduct (pcl::ihs::dome.getVertices ()).maxCoeff (&index); + aligned_direction.transpose() + .lazyProduct(pcl::ihs::dome.getVertices()) + .maxCoeff(&index); // Set the observed direction bit at 'index' // http://stackoverflow.com/questions/47981/how-do-you-set-clear-and-toggle-a-single-bit-in-c/47990#47990 @@ -148,7 +148,7 @@ pcl::ihs::addDirection (const Eigen::Vector4f& normal, //////////////////////////////////////////////////////////////////////////////// unsigned int -pcl::ihs::countDirections (const std::uint32_t directions) +pcl::ihs::countDirections(const std::uint32_t directions) { // http://stackoverflow.com/questions/109023/best-algorithm-to-count-the-number-of-set-bits-in-a-32-bit-integer/109025#109025 unsigned int i = directions - ((directions >> 1) & 0x55555555); diff --git a/apps/include/pcl/apps/manual_registration.h b/apps/include/pcl/apps/manual_registration.h index d9fffbdb..466c2306 100644 --- a/apps/include/pcl/apps/manual_registration.h +++ b/apps/include/pcl/apps/manual_registration.h @@ -78,7 +78,7 @@ public: ManualRegistration(); - ~ManualRegistration() {} + ~ManualRegistration() override = default; void setSrcCloud(pcl::PointCloud::Ptr cloud_src) diff --git a/apps/include/pcl/apps/nn_classification.h b/apps/include/pcl/apps/nn_classification.h index 19bbd0ec..d1dc06da 100644 --- a/apps/include/pcl/apps/nn_classification.h +++ b/apps/include/pcl/apps/nn_classification.h @@ -42,7 +42,8 @@ #include #include -#include // for FLT_MAX +#include +#include // for std::map namespace pcl { @@ -80,7 +81,7 @@ public: { // Do not limit the number of dimensions used in the tree typename pcl::CustomPointRepresentation::Ptr cpr( - new pcl::CustomPointRepresentation(INT_MAX, 0)); + new pcl::CustomPointRepresentation(std::numeric_limits::max(), 0)); tree_.reset(new pcl::KdTreeFLANN); tree_->setPointRepresentation(cpr); tree_->setInputCloud(features); @@ -191,7 +192,10 @@ public: * \return pair of label and score for each training class from the neighborhood */ ResultPtr - classify(const PointT& p_q, double radius, float gaussian_param, int max_nn = INT_MAX) + classify(const PointT& p_q, + double radius, + float gaussian_param, + int max_nn = std::numeric_limits::max()) { pcl::Indices k_indices; std::vector k_sqr_distances; @@ -234,7 +238,7 @@ public: double radius, pcl::Indices& k_indices, std::vector& k_sqr_distances, - int max_nn = INT_MAX) + int max_nn = std::numeric_limits::max()) { return tree_->radiusSearch(p_q, radius, k_indices, k_sqr_distances, max_nn); } @@ -250,7 +254,8 @@ public: std::vector& k_sqr_distances) { // Reserve space for distances - auto sqr_distances = std::make_shared>(classes_.size(), FLT_MAX); + auto sqr_distances = std::make_shared>( + classes_.size(), std::numeric_limits::max()); // Select square distance to each class for (auto i = k_indices.cbegin(); i != k_indices.cend(); ++i) @@ -282,7 +287,7 @@ public: for (std::vector::const_iterator it = sqr_distances->begin(); it != sqr_distances->end(); ++it) - if (*it != FLT_MAX) { + if (*it != std::numeric_limits::max()) { result->first.push_back(classes_[it - sqr_distances->begin()]); result->second.push_back(sqrt(*it)); sum_dist += result->second.back(); @@ -320,7 +325,7 @@ public: for (std::vector::const_iterator it = sqr_distances->begin(); it != sqr_distances->end(); ++it) - if (*it != FLT_MAX) { + if (*it != std::numeric_limits::max()) { result->first.push_back(classes_[it - sqr_distances->begin()]); // TODO leave it squared, and relate param to sigma... result->second.push_back(std::exp(-std::sqrt(*it) / gaussian_param)); diff --git a/apps/include/pcl/apps/openni_passthrough.h b/apps/include/pcl/apps/openni_passthrough.h index 941a49a7..94087404 100644 --- a/apps/include/pcl/apps/openni_passthrough.h +++ b/apps/include/pcl/apps/openni_passthrough.h @@ -76,7 +76,7 @@ public: OpenNIPassthrough(pcl::OpenNIGrabber& grabber); - ~OpenNIPassthrough() + ~OpenNIPassthrough() override { if (grabber_.isRunning()) grabber_.stop(); diff --git a/apps/include/pcl/apps/organized_segmentation_demo.h b/apps/include/pcl/apps/organized_segmentation_demo.h index b83809cc..5ba08a44 100644 --- a/apps/include/pcl/apps/organized_segmentation_demo.h +++ b/apps/include/pcl/apps/organized_segmentation_demo.h @@ -84,7 +84,7 @@ public: OrganizedSegmentationDemo(pcl::Grabber& grabber); - ~OrganizedSegmentationDemo() + ~OrganizedSegmentationDemo() override { if (grabber_.isRunning()) grabber_.stop(); diff --git a/apps/include/pcl/apps/pcd_video_player.h b/apps/include/pcl/apps/pcd_video_player.h index 2f5e4a19..ba177d66 100644 --- a/apps/include/pcl/apps/pcd_video_player.h +++ b/apps/include/pcl/apps/pcd_video_player.h @@ -83,7 +83,7 @@ public: PCDVideoPlayer(); - ~PCDVideoPlayer() {} + ~PCDVideoPlayer() override = default; protected: void diff --git a/apps/modeler/CMakeLists.txt b/apps/modeler/CMakeLists.txt index aa616052..7c621248 100644 --- a/apps/modeler/CMakeLists.txt +++ b/apps/modeler/CMakeLists.txt @@ -19,7 +19,7 @@ if(${DEFAULT} STREQUAL "TRUE") endif() PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSUBSYS_NAME} PARENT_NAME ${SUBSYS_NAME} DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) PCL_ADD_DOC("${SUBSUBSYS_NAME}") diff --git a/apps/modeler/include/pcl/apps/modeler/abstract_item.h b/apps/modeler/include/pcl/apps/modeler/abstract_item.h index 07a023e1..0153d529 100755 --- a/apps/modeler/include/pcl/apps/modeler/abstract_item.h +++ b/apps/modeler/include/pcl/apps/modeler/abstract_item.h @@ -53,7 +53,6 @@ class ParameterDialog; class AbstractItem { public: AbstractItem(); - ~AbstractItem(); void showContextMenu(const QPoint* position); diff --git a/apps/modeler/include/pcl/apps/modeler/cloud_mesh.h b/apps/modeler/include/pcl/apps/modeler/cloud_mesh.h index cc364bdd..aa1ee3a4 100755 --- a/apps/modeler/include/pcl/apps/modeler/cloud_mesh.h +++ b/apps/modeler/include/pcl/apps/modeler/cloud_mesh.h @@ -59,7 +59,6 @@ public: CloudMesh(); CloudMesh(PointCloudPtr cloud); - ~CloudMesh(); PointCloudPtr& getCloud() diff --git a/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item.h b/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item.h index 02345010..62b8741e 100755 --- a/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item.h +++ b/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item.h @@ -52,7 +52,6 @@ public: CloudMeshItem(QTreeWidgetItem* parent, const std::string& filename); CloudMeshItem(QTreeWidgetItem* parent, CloudMesh::PointCloudPtr cloud); CloudMeshItem(QTreeWidgetItem* parent, const CloudMeshItem& cloud_mesh_item); - ~CloudMeshItem(); inline CloudMesh::Ptr& getCloudMesh() diff --git a/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item_updater.h b/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item_updater.h index 136dd6b4..bc98808f 100755 --- a/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item_updater.h +++ b/apps/modeler/include/pcl/apps/modeler/cloud_mesh_item_updater.h @@ -48,7 +48,6 @@ class CloudMeshItemUpdater : public QObject { public: CloudMeshItemUpdater(CloudMeshItem* cloud_mesh_item); - ~CloudMeshItemUpdater(); public Q_SLOTS: void diff --git a/apps/modeler/include/pcl/apps/modeler/dock_widget.h b/apps/modeler/include/pcl/apps/modeler/dock_widget.h index 40d63a34..4f6fec1c 100755 --- a/apps/modeler/include/pcl/apps/modeler/dock_widget.h +++ b/apps/modeler/include/pcl/apps/modeler/dock_widget.h @@ -48,7 +48,6 @@ public: Qt::WindowFlags flags = Qt::WindowFlags()); explicit DockWidget(QWidget* parent = nullptr, Qt::WindowFlags flags = Qt::WindowFlags()); - ~DockWidget(); void setFocusBasedStyle(bool focused); diff --git a/apps/modeler/include/pcl/apps/modeler/icp_registration_worker.h b/apps/modeler/include/pcl/apps/modeler/icp_registration_worker.h index 53c30600..3ae287f6 100755 --- a/apps/modeler/include/pcl/apps/modeler/icp_registration_worker.h +++ b/apps/modeler/include/pcl/apps/modeler/icp_registration_worker.h @@ -50,7 +50,6 @@ public: ICPRegistrationWorker(CloudMesh::PointCloudPtr cloud, const QList& cloud_mesh_items, QWidget* parent = nullptr); - ~ICPRegistrationWorker(); protected: std::string diff --git a/apps/modeler/include/pcl/apps/modeler/normals_actor_item.h b/apps/modeler/include/pcl/apps/modeler/normals_actor_item.h index 94e3221f..e974a9d6 100755 --- a/apps/modeler/include/pcl/apps/modeler/normals_actor_item.h +++ b/apps/modeler/include/pcl/apps/modeler/normals_actor_item.h @@ -49,7 +49,6 @@ public: NormalsActorItem(QTreeWidgetItem* parent, const CloudMesh::Ptr& cloud_mesh, const vtkSmartPointer& render_window); - ~NormalsActorItem(); std::string getItemName() const override diff --git a/apps/modeler/include/pcl/apps/modeler/parameter.h b/apps/modeler/include/pcl/apps/modeler/parameter.h index 58a12e98..0fa59fb9 100755 --- a/apps/modeler/include/pcl/apps/modeler/parameter.h +++ b/apps/modeler/include/pcl/apps/modeler/parameter.h @@ -57,7 +57,7 @@ public: const boost::any& value) : name_(name), description_(description), default_value_(value), current_value_(value) {} - ~Parameter() {} + virtual ~Parameter() = default; const std::string& getName() const @@ -113,7 +113,6 @@ public: BoolParameter(const std::string& name, const std::string& description, bool value) : Parameter(name, description, value) {} - ~BoolParameter() {} operator bool() const { return boost::any_cast(current_value_); } @@ -144,7 +143,6 @@ public: int step = 1) : Parameter(name, description, value), low_(low), high_(high), step_(step) {} - virtual ~IntParameter() {} operator int() const { return boost::any_cast(current_value_); } @@ -196,7 +194,6 @@ public: const std::map& candidates) : Parameter(name, description, value), candidates_(candidates) {} - ~EnumParameter() {} operator T() const { return boost::any_cast(current_value_); } @@ -229,7 +226,6 @@ public: double step = 0.01) : Parameter(name, description, value), low_(low), high_(high), step_(step) {} - virtual ~DoubleParameter() {} operator double() const { return boost::any_cast(current_value_); } @@ -279,7 +275,6 @@ public: const QColor& value) : Parameter(name, description, value) {} - ~ColorParameter() {} operator QColor() const { return boost::any_cast(current_value_); } diff --git a/apps/modeler/include/pcl/apps/modeler/parameter_dialog.h b/apps/modeler/include/pcl/apps/modeler/parameter_dialog.h index 6ee5f669..6ebb1554 100755 --- a/apps/modeler/include/pcl/apps/modeler/parameter_dialog.h +++ b/apps/modeler/include/pcl/apps/modeler/parameter_dialog.h @@ -51,7 +51,6 @@ public: ParameterModel(int rows, int columns, QObject* parent = nullptr) : QStandardItemModel(rows, columns, parent) {} - ~ParameterModel() {} Qt::ItemFlags flags(const QModelIndex& index) const override @@ -65,7 +64,6 @@ class ParameterDialog : public QDialog { Q_OBJECT public: ParameterDialog(const std::string& title, QWidget* parent = nullptr); - ~ParameterDialog() {} void addParameter(Parameter* parameter); diff --git a/apps/modeler/include/pcl/apps/modeler/points_actor_item.h b/apps/modeler/include/pcl/apps/modeler/points_actor_item.h index 9c08e6c7..be9bafa0 100755 --- a/apps/modeler/include/pcl/apps/modeler/points_actor_item.h +++ b/apps/modeler/include/pcl/apps/modeler/points_actor_item.h @@ -49,7 +49,6 @@ public: PointsActorItem(QTreeWidgetItem* parent, const CloudMesh::Ptr& cloud_mesh, const vtkSmartPointer& render_window); - ~PointsActorItem(); std::string getItemName() const override diff --git a/apps/modeler/include/pcl/apps/modeler/render_window.h b/apps/modeler/include/pcl/apps/modeler/render_window.h index 0413e85b..7e5cb466 100755 --- a/apps/modeler/include/pcl/apps/modeler/render_window.h +++ b/apps/modeler/include/pcl/apps/modeler/render_window.h @@ -50,7 +50,7 @@ class RenderWindow : public PCLQVTKWidget { public: RenderWindow(RenderWindowItem* render_window_item, QWidget* parent = nullptr, - Qt::WindowFlags flags = nullptr); + Qt::WindowFlags flags = {}); ~RenderWindow(); QSize diff --git a/apps/modeler/include/pcl/apps/modeler/scene_tree.h b/apps/modeler/include/pcl/apps/modeler/scene_tree.h index 768761d6..31b00f0d 100755 --- a/apps/modeler/include/pcl/apps/modeler/scene_tree.h +++ b/apps/modeler/include/pcl/apps/modeler/scene_tree.h @@ -49,7 +49,6 @@ class SceneTree : public QTreeWidget { public: SceneTree(QWidget* parent = nullptr); - ~SceneTree(); QSize sizeHint() const override; diff --git a/apps/modeler/include/pcl/apps/modeler/surface_actor_item.h b/apps/modeler/include/pcl/apps/modeler/surface_actor_item.h index 2ca3df23..af096b2c 100755 --- a/apps/modeler/include/pcl/apps/modeler/surface_actor_item.h +++ b/apps/modeler/include/pcl/apps/modeler/surface_actor_item.h @@ -58,7 +58,6 @@ public: SurfaceActorItem(QTreeWidgetItem* parent, const CloudMesh::Ptr& cloud_mesh, const vtkSmartPointer& render_window); - ~SurfaceActorItem(); std::string getItemName() const override diff --git a/apps/modeler/src/abstract_item.cpp b/apps/modeler/src/abstract_item.cpp index 9c022c39..e3d337f4 100755 --- a/apps/modeler/src/abstract_item.cpp +++ b/apps/modeler/src/abstract_item.cpp @@ -41,9 +41,6 @@ ////////////////////////////////////////////////////////////////////////////////////////////// pcl::modeler::AbstractItem::AbstractItem() {} -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::AbstractItem::~AbstractItem() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::AbstractItem::showContextMenu(const QPoint* position) diff --git a/apps/modeler/src/cloud_mesh.cpp b/apps/modeler/src/cloud_mesh.cpp index c7936b3b..5d350b80 100755 --- a/apps/modeler/src/cloud_mesh.cpp +++ b/apps/modeler/src/cloud_mesh.cpp @@ -64,9 +64,6 @@ pcl::modeler::CloudMesh::CloudMesh(PointCloudPtr cloud) updateVtkPoints(); } -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::CloudMesh::~CloudMesh() {} - ////////////////////////////////////////////////////////////////////////////////////////////// std::vector pcl::modeler::CloudMesh::getAvaiableFieldNames() const diff --git a/apps/modeler/src/cloud_mesh_item.cpp b/apps/modeler/src/cloud_mesh_item.cpp index e40ddbf5..1ac45a5b 100755 --- a/apps/modeler/src/cloud_mesh_item.cpp +++ b/apps/modeler/src/cloud_mesh_item.cpp @@ -107,9 +107,6 @@ pcl::modeler::CloudMeshItem::CloudMeshItem(QTreeWidgetItem* parent, treeWidget()->expandItem(this); } -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::CloudMeshItem::~CloudMeshItem() {} - ////////////////////////////////////////////////////////////////////////////////////////////// bool pcl::modeler::CloudMeshItem::savePointCloud(const QList& items, diff --git a/apps/modeler/src/cloud_mesh_item_updater.cpp b/apps/modeler/src/cloud_mesh_item_updater.cpp index 9f933ca1..0cf9be5f 100755 --- a/apps/modeler/src/cloud_mesh_item_updater.cpp +++ b/apps/modeler/src/cloud_mesh_item_updater.cpp @@ -42,9 +42,6 @@ pcl::modeler::CloudMeshItemUpdater::CloudMeshItemUpdater(CloudMeshItem* cloud_me : cloud_mesh_item_(cloud_mesh_item) {} -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::CloudMeshItemUpdater::~CloudMeshItemUpdater() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::CloudMeshItemUpdater::updateCloudMeshItem() diff --git a/apps/modeler/src/dock_widget.cpp b/apps/modeler/src/dock_widget.cpp index ae73f9d2..893a1da5 100755 --- a/apps/modeler/src/dock_widget.cpp +++ b/apps/modeler/src/dock_widget.cpp @@ -56,9 +56,6 @@ pcl::modeler::DockWidget::DockWidget(QWidget* parent, Qt::WindowFlags flags) setFocusPolicy(Qt::StrongFocus); } -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::DockWidget::~DockWidget() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::DockWidget::focusInEvent(QFocusEvent* event) diff --git a/apps/modeler/src/icp_registration_worker.cpp b/apps/modeler/src/icp_registration_worker.cpp index 7628ed81..2210d18c 100755 --- a/apps/modeler/src/icp_registration_worker.cpp +++ b/apps/modeler/src/icp_registration_worker.cpp @@ -61,9 +61,6 @@ pcl::modeler::ICPRegistrationWorker::ICPRegistrationWorker( , euclidean_fitness_epsilon_(nullptr) {} -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::ICPRegistrationWorker::~ICPRegistrationWorker() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::ICPRegistrationWorker::initParameters(CloudMeshItem* cloud_mesh_item) diff --git a/apps/modeler/src/normals_actor_item.cpp b/apps/modeler/src/normals_actor_item.cpp index 6530c560..a594127d 100644 --- a/apps/modeler/src/normals_actor_item.cpp +++ b/apps/modeler/src/normals_actor_item.cpp @@ -55,9 +55,6 @@ pcl::modeler::NormalsActorItem::NormalsActorItem( , scale_(0.1) {} -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::NormalsActorItem::~NormalsActorItem() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::NormalsActorItem::createNormalLines() diff --git a/apps/modeler/src/points_actor_item.cpp b/apps/modeler/src/points_actor_item.cpp index c9adf062..96711887 100755 --- a/apps/modeler/src/points_actor_item.cpp +++ b/apps/modeler/src/points_actor_item.cpp @@ -52,9 +52,6 @@ pcl::modeler::PointsActorItem::PointsActorItem( parent, cloud_mesh, render_window, vtkSmartPointer::New(), "Points") {} -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::PointsActorItem::~PointsActorItem() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::PointsActorItem::initImpl() @@ -80,9 +77,6 @@ pcl::modeler::PointsActorItem::initImpl() mapper->SetScalarModeToUsePointData(); mapper->InterpolateScalarsBeforeMappingOn(); mapper->ScalarVisibilityOn(); -#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2 - mapper->ImmediateModeRenderingOff(); -#endif vtkSmartPointer actor = vtkSmartPointer(dynamic_cast(actor_.GetPointer())); diff --git a/apps/modeler/src/scene_tree.cpp b/apps/modeler/src/scene_tree.cpp index c923ee3b..69919d50 100755 --- a/apps/modeler/src/scene_tree.cpp +++ b/apps/modeler/src/scene_tree.cpp @@ -78,9 +78,6 @@ pcl::modeler::SceneTree::SceneTree(QWidget* parent) : QTreeWidget(parent) SLOT(slotOnItemDoubleClicked(QTreeWidgetItem*))); } -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::SceneTree::~SceneTree() {} - ////////////////////////////////////////////////////////////////////////////////////////////// QSize pcl::modeler::SceneTree::sizeHint() const @@ -176,15 +173,13 @@ pcl::modeler::SceneTree::slotOpenPointCloud() closePointCloud(cloud_mesh_items); } - for (QStringList::const_iterator filenames_it = filenames.begin(); - filenames_it != filenames.end(); - ++filenames_it) { - if (!openPointCloud(*filenames_it)) + for (const auto& filename : filenames) { + if (!openPointCloud(filename)) QMessageBox::warning(main_window, tr("Failed to Open Point Cloud"), tr("Can not open point cloud file %1, please check if it's " "in valid .pcd format!") - .arg(*filenames_it)); + .arg(filename)); } } @@ -209,14 +204,13 @@ pcl::modeler::SceneTree::slotImportPointCloud() if (filenames.isEmpty()) return; - for (QStringList::const_iterator filenames_it = filenames.begin(); - filenames_it != filenames.end(); - ++filenames_it) { - if (!openPointCloud(*filenames_it)) + for (const auto& filename : filenames) { + if (!openPointCloud(filename)) { QMessageBox::warning( main_window, tr("Failed to Import Point Cloud"), - tr("Can not import point cloud file %1 as .pcd file!").arg(*filenames_it)); + tr("Can not import point cloud file %1 as .pcd file!").arg(filename)); + } } } @@ -260,11 +254,8 @@ pcl::modeler::SceneTree::closePointCloud(const QList& items) delete item; } - for (QList::const_iterator render_window_items_it = - render_window_items.begin(); - render_window_items_it != render_window_items.end(); - ++render_window_items_it) { - (*render_window_items_it)->getRenderWindow()->render(); + for (const auto& render_window_item : render_window_items) { + render_window_item->getRenderWindow()->render(); } } @@ -384,10 +375,8 @@ pcl::modeler::SceneTree::slotUpdateOnSelectionChange(const QItemSelection& selec const QItemSelection& deselected) { QModelIndexList selected_indices = selected.indexes(); - for (QModelIndexList::const_iterator selected_indices_it = selected_indices.begin(); - selected_indices_it != selected_indices.end(); - ++selected_indices_it) { - QTreeWidgetItem* item = itemFromIndex(*selected_indices_it); + for (const auto& selected_index : selected_indices) { + QTreeWidgetItem* item = itemFromIndex(selected_index); RenderWindowItem* render_window_item = dynamic_cast(item); if (render_window_item != nullptr) { render_window_item->getRenderWindow()->setActive(true); @@ -395,12 +384,9 @@ pcl::modeler::SceneTree::slotUpdateOnSelectionChange(const QItemSelection& selec } QModelIndexList deselected_indices = deselected.indexes(); - for (QModelIndexList::const_iterator deselected_indices_it = - deselected_indices.begin(); - deselected_indices_it != deselected_indices.end(); - ++deselected_indices_it) { - QTreeWidgetItem* item = itemFromIndex(*deselected_indices_it); - RenderWindowItem* render_window_item = dynamic_cast(item); + for (const auto& deselected_index : deselected_indices) { + QTreeWidgetItem* item = itemFromIndex(deselected_index); + auto* render_window_item = dynamic_cast(item); if (render_window_item != nullptr) { render_window_item->getRenderWindow()->setActive(false); } diff --git a/apps/modeler/src/surface_actor_item.cpp b/apps/modeler/src/surface_actor_item.cpp index 067aa566..aa5aa233 100755 --- a/apps/modeler/src/surface_actor_item.cpp +++ b/apps/modeler/src/surface_actor_item.cpp @@ -53,9 +53,6 @@ pcl::modeler::SurfaceActorItem::SurfaceActorItem( parent, cloud_mesh, render_window, vtkSmartPointer::New(), "Surface") {} -////////////////////////////////////////////////////////////////////////////////////////////// -pcl::modeler::SurfaceActorItem::~SurfaceActorItem() {} - ////////////////////////////////////////////////////////////////////////////////////////////// void pcl::modeler::SurfaceActorItem::initImpl() @@ -77,9 +74,6 @@ pcl::modeler::SurfaceActorItem::initImpl() mapper->SetScalarModeToUsePointData(); mapper->InterpolateScalarsBeforeMappingOn(); mapper->ScalarVisibilityOn(); -#if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2 - mapper->ImmediateModeRenderingOff(); -#endif vtkSmartPointer actor = vtkSmartPointer(dynamic_cast(actor_.GetPointer())); diff --git a/apps/point_cloud_editor/CMakeLists.txt b/apps/point_cloud_editor/CMakeLists.txt index e5321278..d46b49f5 100644 --- a/apps/point_cloud_editor/CMakeLists.txt +++ b/apps/point_cloud_editor/CMakeLists.txt @@ -8,8 +8,8 @@ if(${DEFAULT} STREQUAL "TRUE") set(DEFAULT FALSE) endif() -PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSUBSYS_DEPEND(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) +PCL_SUBSUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSUBSYS_NAME}" "${SUBSUBSYS_DESC}" ${DEFAULT} "${REASON}") +PCL_SUBSYS_DEPEND(build NAME ${SUBSUBSYS_NAME} PARENT_NAME ${SUBSYS_NAME} DEPS ${SUBSUBSYS_DEPS} EXT_DEPS ${SUBSUBSYS_EXT_DEPS}) PCL_ADD_DOC(${SUBSUBSYS_NAME}) if(NOT build) diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudEditorWidget.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudEditorWidget.h index de5b3b77..7630d224 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudEditorWidget.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudEditorWidget.h @@ -69,7 +69,7 @@ class CloudEditorWidget : public QGLWidget CloudEditorWidget (QWidget *parent = nullptr); /// @brief Destructor - ~CloudEditorWidget (); + ~CloudEditorWidget () override; /// @brief Attempts to load the point cloud designated by the passed file /// name. diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudTransformTool.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudTransformTool.h index 101556d5..66ae1b48 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudTransformTool.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cloudTransformTool.h @@ -55,7 +55,7 @@ class CloudTransformTool : public ToolInterface CloudTransformTool (CloudPtr cloud_ptr); /// @brief Destructor - ~CloudTransformTool (); + ~CloudTransformTool () override; /// @brief Initialize the current transform with mouse screen coordinates /// and key modifiers. diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/command.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/command.h index 35fd9cb7..e7dbdedd 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/command.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/command.h @@ -50,8 +50,7 @@ class Command public: /// @brief Destructor virtual ~Command () - { - } + = default; protected: /// Allows command queues to be the only objects which are able to execute diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/commandQueue.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/commandQueue.h index a0e51f14..93433339 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/commandQueue.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/commandQueue.h @@ -65,8 +65,7 @@ class CommandQueue /// @brief Destructor ~CommandQueue () - { - } + = default; /// @brief Executes a command. If the command has an undo function, then /// adds the command to the queue. diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h index 438f1134..6f1e0b81 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/cutCommand.h @@ -69,7 +69,7 @@ class CutCommand : public Command operator= (const CutCommand&) = delete; /// @brief Destructor - ~CutCommand (); + ~CutCommand () override; protected: /// @brief Moves the selected points to the copy buffer and removes them diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/denoiseParameterForm.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/denoiseParameterForm.h index 0f7c1fb7..7cd7e316 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/denoiseParameterForm.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/denoiseParameterForm.h @@ -56,7 +56,7 @@ class DenoiseParameterForm : public QDialog DenoiseParameterForm(); /// @brief Destructor - ~DenoiseParameterForm (); + ~DenoiseParameterForm () override; /// @brief Returns the mean inline diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h index a66d6471..bfa2e997 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/mainWindow.h @@ -73,7 +73,7 @@ class MainWindow : public QMainWindow MainWindow (int argc, char **argv); /// @brief Destructor - ~MainWindow (); + ~MainWindow () override; /// @brief Increase the value of the spinbox by 1. void diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select1DTool.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select1DTool.h index 679e97bd..fc21d80a 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select1DTool.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select1DTool.h @@ -58,9 +58,8 @@ class Select1DTool : public ToolInterface Select1DTool (SelectionPtr selection_ptr, CloudPtr cloud_ptr); /// @brief Destructor - ~Select1DTool () - { - } + ~Select1DTool () override + = default; /// @brief Does nothing for 1D selection. /// @sa end diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select2DTool.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select2DTool.h index aacbbfbe..985ec4e1 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select2DTool.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/select2DTool.h @@ -60,7 +60,7 @@ class Select2DTool : public ToolInterface Select2DTool (SelectionPtr selection_ptr, CloudPtr cloud_ptr); /// @brief Destructor - ~Select2DTool (); + ~Select2DTool () override; /// @brief Initializes the selection tool with the initial mouse screen /// coordinates and key modifiers. The passed coordinates are used for diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selectionTransformTool.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selectionTransformTool.h index 976d1e98..d871a44a 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selectionTransformTool.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/selectionTransformTool.h @@ -69,9 +69,8 @@ class SelectionTransformTool : public ToolInterface CommandQueuePtr command_queue_ptr); /// @brief Destructor - ~SelectionTransformTool () - { - } + ~SelectionTransformTool () override + = default; /// @brief Initialize the transform tool with mouse screen coordinates /// and key modifiers. diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statistics.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statistics.h index e1b866bb..8618e4b9 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statistics.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statistics.h @@ -49,8 +49,7 @@ class Statistics public: /// @brief Destructor virtual ~Statistics () - { - } + = default; /// @brief Returns the strings of the statistics. static @@ -64,8 +63,7 @@ class Statistics protected: /// @brief The default constructor. Statistics () - { - } + = default; /// @brief Copy Constructor Statistics (const Statistics&) diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statisticsDialog.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statisticsDialog.h index eb28cfd8..59ee72f4 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statisticsDialog.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/statisticsDialog.h @@ -58,7 +58,7 @@ class StatisticsDialog : public QDialog /// @brief Default Constructor StatisticsDialog(QWidget *parent = nullptr); /// @brief Destructor - ~StatisticsDialog (); + ~StatisticsDialog () override; public Q_SLOTS: /// @brief update the dialog box. diff --git a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/toolInterface.h b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/toolInterface.h index f6348796..c5cc67c8 100644 --- a/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/toolInterface.h +++ b/apps/point_cloud_editor/include/pcl/apps/point_cloud_editor/toolInterface.h @@ -49,8 +49,7 @@ class ToolInterface public: /// @brief Destructor. virtual ~ToolInterface () - { - } + = default; /// @brief set the initial state of the tool from the screen coordinates /// of the mouse as well as the value of the modifier. @@ -102,8 +101,7 @@ class ToolInterface protected: /// @brief Default constructor ToolInterface () - { - } + = default; private: /// @brief Copy constructor - tools are non-copyable diff --git a/apps/point_cloud_editor/src/cloudEditorWidget.cpp b/apps/point_cloud_editor/src/cloudEditorWidget.cpp index b9150f7b..6a8d50f6 100644 --- a/apps/point_cloud_editor/src/cloudEditorWidget.cpp +++ b/apps/point_cloud_editor/src/cloudEditorWidget.cpp @@ -85,8 +85,7 @@ CloudEditorWidget::CloudEditorWidget (QWidget *parent) } CloudEditorWidget::~CloudEditorWidget () -{ -} += default; void CloudEditorWidget::loadFile(const std::string &filename) diff --git a/apps/point_cloud_editor/src/cloudTransformTool.cpp b/apps/point_cloud_editor/src/cloudTransformTool.cpp index b0a493f6..4a875de5 100644 --- a/apps/point_cloud_editor/src/cloudTransformTool.cpp +++ b/apps/point_cloud_editor/src/cloudTransformTool.cpp @@ -53,8 +53,7 @@ CloudTransformTool::CloudTransformTool (CloudPtr cloud_ptr) } CloudTransformTool::~CloudTransformTool () -{ -} += default; void CloudTransformTool::start (int x, int y, BitMask, BitMask) diff --git a/apps/point_cloud_editor/src/common.cpp b/apps/point_cloud_editor/src/common.cpp index 08c79e67..b9511b52 100644 --- a/apps/point_cloud_editor/src/common.cpp +++ b/apps/point_cloud_editor/src/common.cpp @@ -64,7 +64,7 @@ multMatrix(const float* left, const float* right, float* result) r[i * MATRIX_SIZE_DIM + j] = sum; } } - std::copy(r, r+MATRIX_SIZE, result); + std::copy(r, r + MATRIX_SIZE, result); } diff --git a/apps/point_cloud_editor/src/cutCommand.cpp b/apps/point_cloud_editor/src/cutCommand.cpp index a184d50b..4cc74646 100644 --- a/apps/point_cloud_editor/src/cutCommand.cpp +++ b/apps/point_cloud_editor/src/cutCommand.cpp @@ -51,8 +51,7 @@ CutCommand::CutCommand (CopyBufferPtr copy_buffer_ptr, } CutCommand::~CutCommand () -{ -} += default; void CutCommand::execute () diff --git a/apps/point_cloud_editor/src/mainWindow.cpp b/apps/point_cloud_editor/src/mainWindow.cpp index 1835a1cc..3ccb29d0 100644 --- a/apps/point_cloud_editor/src/mainWindow.cpp +++ b/apps/point_cloud_editor/src/mainWindow.cpp @@ -56,9 +56,7 @@ window_width_(WINDOW_WIDTH), window_height_(WINDOW_HEIGHT) } MainWindow::~MainWindow() -{ - -} += default; void MainWindow::about () diff --git a/apps/point_cloud_editor/src/select2DTool.cpp b/apps/point_cloud_editor/src/select2DTool.cpp index ae30aea9..93e43102 100644 --- a/apps/point_cloud_editor/src/select2DTool.cpp +++ b/apps/point_cloud_editor/src/select2DTool.cpp @@ -54,8 +54,7 @@ Select2DTool::Select2DTool (SelectionPtr selection_ptr, CloudPtr cloud_ptr) } Select2DTool::~Select2DTool () -{ -} += default; void Select2DTool::start (int x, int y, BitMask, BitMask) diff --git a/apps/point_cloud_editor/src/selectionTransformTool.cpp b/apps/point_cloud_editor/src/selectionTransformTool.cpp index e1e09e43..16698d4b 100644 --- a/apps/point_cloud_editor/src/selectionTransformTool.cpp +++ b/apps/point_cloud_editor/src/selectionTransformTool.cpp @@ -196,8 +196,8 @@ SelectionTransformTool::findSelectionCenter () Selection::const_iterator it = selection_ptr_->begin(); Point3D point_3d = cloud_ptr_->getObjectSpacePoint (*it); float *pt = &(point_3d.data[X]); - std::copy(pt, pt+XYZ_SIZE, max_xyz); - std::copy(max_xyz, max_xyz+XYZ_SIZE, min_xyz); + std::copy(pt, pt + XYZ_SIZE, max_xyz); + std::copy(max_xyz, max_xyz + XYZ_SIZE, min_xyz); for (++it; it != selection_ptr_->end(); ++it) { diff --git a/apps/point_cloud_editor/src/trackball.cpp b/apps/point_cloud_editor/src/trackball.cpp index f47732a0..f95f7b08 100644 --- a/apps/point_cloud_editor/src/trackball.cpp +++ b/apps/point_cloud_editor/src/trackball.cpp @@ -48,28 +48,15 @@ TrackBall::TrackBall() : quat_(1.0f), origin_x_(0), origin_y_(0), origin_z_(0) (TRACKBALL_RADIUS_SCALE * static_cast(WINDOW_WIDTH)); } -TrackBall::TrackBall(const TrackBall ©) : - quat_(copy.quat_), origin_x_(copy.origin_x_), origin_y_(copy.origin_y_), - origin_z_(copy.origin_z_), radius_sqr_(copy.radius_sqr_) -{ - -} +TrackBall::TrackBall(const TrackBall ©) += default; TrackBall::~TrackBall() -{ - -} += default; TrackBall& TrackBall::operator=(const TrackBall &rhs) -{ - quat_ = rhs.quat_; - origin_x_ = rhs.origin_x_; - origin_y_ = rhs.origin_y_; - origin_z_ = rhs.origin_z_; - radius_sqr_ = rhs.radius_sqr_; - return *this; -} += default; void TrackBall::start(int s_x, int s_y) diff --git a/apps/point_cloud_editor/src/transformCommand.cpp b/apps/point_cloud_editor/src/transformCommand.cpp index 3143825d..34da2014 100644 --- a/apps/point_cloud_editor/src/transformCommand.cpp +++ b/apps/point_cloud_editor/src/transformCommand.cpp @@ -53,9 +53,9 @@ TransformCommand::TransformCommand(const ConstSelectionPtr& selection_ptr, translate_z_(translate_z) { internal_selection_ptr_ = SelectionPtr(new Selection(*selection_ptr)); - std::copy(matrix, matrix+MATRIX_SIZE, transform_matrix_); + std::copy(matrix, matrix + MATRIX_SIZE, transform_matrix_); const float *cloud_matrix = cloud_ptr_->getMatrix(); - std::copy(cloud_matrix, cloud_matrix+MATRIX_SIZE, cloud_matrix_); + std::copy(cloud_matrix, cloud_matrix + MATRIX_SIZE, cloud_matrix_); invertMatrix(cloud_matrix, cloud_matrix_inv_); cloud_ptr_->getCenter(cloud_center_[X], cloud_center_[Y], cloud_center_[Z]); } diff --git a/apps/src/convolve.cpp b/apps/src/convolve.cpp index 6bf42e7a..b236e609 100644 --- a/apps/src/convolve.cpp +++ b/apps/src/convolve.cpp @@ -92,21 +92,24 @@ main(int argc, char** argv) // user don't need help find convolving direction // convolve row - if (pcl::console::find_switch(argc, argv, "-r")) + if (pcl::console::find_switch(argc, argv, "-r")) { direction = 0; + } else { // convolve column - if (pcl::console::find_switch(argc, argv, "-c")) + if (pcl::console::find_switch(argc, argv, "-c")) { direction = 1; - else - // convolve both - if (pcl::console::find_switch(argc, argv, "-s")) - direction = 2; - else { - // wrong direction given print usage - usage(argv); - return 1; } + else + // convolve both + if (pcl::console::find_switch(argc, argv, "-s")) { + direction = 2; + } + else { + // wrong direction given print usage + usage(argv); + return 1; + } } // number of threads if any diff --git a/apps/src/grabcut_2d.cpp b/apps/src/grabcut_2d.cpp index d383f55a..02b3faec 100644 --- a/apps/src/grabcut_2d.cpp +++ b/apps/src/grabcut_2d.cpp @@ -38,7 +38,7 @@ public: : pcl::GrabCut(K, lambda) {} - ~GrabCutHelper() {} + ~GrabCutHelper() override = default; void setInputCloud(const pcl::PointCloud::ConstPtr& cloud) override; @@ -177,7 +177,7 @@ void GrabCutHelper::buildImages() { using namespace pcl::segmentation::grabcut; - memset(&(*n_links_image_)[0], 0, sizeof(float) * n_links_image_->size()); + std::fill(n_links_image_->begin(), n_links_image_->end(), 0.0f); for (int y = 0; y < static_cast(image_->height); ++y) { for (int x = 0; x < static_cast(image_->width); ++x) { std::size_t index = y * image_->width + x; diff --git a/apps/src/manual_registration/manual_registration.cpp b/apps/src/manual_registration/manual_registration.cpp index 8e4c4699..8dff3b9b 100644 --- a/apps/src/manual_registration/manual_registration.cpp +++ b/apps/src/manual_registration/manual_registration.cpp @@ -45,6 +45,12 @@ #include #include #include + +#include +#if VTK_MAJOR_VERSION >= 9 || (VTK_MAJOR_VERSION == 8 && VTK_MINOR_VERSION >= 2) +#define HAS_QVTKOPENGLWINDOW_H +#include +#endif #include #include @@ -318,6 +324,9 @@ print_usage() int main(int argc, char** argv) { +#ifdef HAS_QVTKOPENGLWINDOW_H + QSurfaceFormat::setDefaultFormat(QVTKOpenGLWindow::defaultFormat()); +#endif QApplication app(argc, argv); pcl::PointCloud::Ptr cloud_src( diff --git a/apps/src/openni_3d_concave_hull.cpp b/apps/src/openni_3d_concave_hull.cpp index d42bf142..822f7a81 100644 --- a/apps/src/openni_3d_concave_hull.cpp +++ b/apps/src/openni_3d_concave_hull.cpp @@ -131,7 +131,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb(cloud); @@ -195,15 +195,15 @@ main(int argc, char** argv) return 1; } - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { PCL_INFO("PointXYZRGBA mode enabled.\n"); - OpenNI3DConcaveHull v(""); + OpenNI3DConcaveHull v(arg); v.run(); } else { PCL_INFO("PointXYZ mode enabled.\n"); - OpenNI3DConcaveHull v(""); + OpenNI3DConcaveHull v(arg); v.run(); } return 0; diff --git a/apps/src/openni_3d_convex_hull.cpp b/apps/src/openni_3d_convex_hull.cpp index 45957b2a..841e429a 100644 --- a/apps/src/openni_3d_convex_hull.cpp +++ b/apps/src/openni_3d_convex_hull.cpp @@ -129,7 +129,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb(cloud); @@ -193,15 +193,15 @@ main(int argc, char** argv) return 1; } - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { PCL_INFO("PointXYZRGBA mode enabled.\n"); - OpenNI3DConvexHull v(""); + OpenNI3DConvexHull v(arg); v.run(); } else { PCL_INFO("PointXYZ mode enabled.\n"); - OpenNI3DConvexHull v(""); + OpenNI3DConvexHull v(arg); v.run(); } return 0; diff --git a/apps/src/openni_boundary_estimation.cpp b/apps/src/openni_boundary_estimation.cpp index cc93ef47..89467dd0 100644 --- a/apps/src/openni_boundary_estimation.cpp +++ b/apps/src/openni_boundary_estimation.cpp @@ -144,7 +144,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function::ConstPtr&)> f = [this](const pcl::PointCloud::ConstPtr& cloud) { @@ -210,9 +210,9 @@ main(int argc, char** argv) return 1; } - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { - OpenNIIntegralImageNormalEstimation v(""); + OpenNIIntegralImageNormalEstimation v(arg); v.run(); } else diff --git a/apps/src/openni_change_viewer.cpp b/apps/src/openni_change_viewer.cpp index b07a4e86..b34786fa 100644 --- a/apps/src/openni_change_viewer.cpp +++ b/apps/src/openni_change_viewer.cpp @@ -111,7 +111,7 @@ public: void run() { - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; std::function::ConstPtr&)> f = [this](const pcl::PointCloud::ConstPtr& cloud) { diff --git a/apps/src/openni_fast_mesh.cpp b/apps/src/openni_fast_mesh.cpp index 05efbe6c..63b1026c 100644 --- a/apps/src/openni_fast_mesh.cpp +++ b/apps/src/openni_fast_mesh.cpp @@ -100,7 +100,7 @@ public: void run(int argc, char** argv) { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb(cloud); @@ -189,15 +189,15 @@ main(int argc, char** argv) return 1; } - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { PCL_INFO("PointXYZRGBA mode enabled.\n"); - OpenNIFastMesh v(""); + OpenNIFastMesh v(arg); v.run(argc, argv); } else { PCL_INFO("PointXYZ mode enabled.\n"); - OpenNIFastMesh v(""); + OpenNIFastMesh v(arg); v.run(argc, argv); } return 0; diff --git a/apps/src/openni_feature_persistence.cpp b/apps/src/openni_feature_persistence.cpp index 17ac9195..70b99cb6 100644 --- a/apps/src/openni_feature_persistence.cpp +++ b/apps/src/openni_feature_persistence.cpp @@ -188,7 +188,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb(cloud); @@ -266,6 +266,10 @@ main(int argc, char** argv) "MultiscaleFeaturePersistence class using the FPFH features\n" << "Use \"-h\" to get more info about the available options.\n"; + std::string arg = ""; + if ((argc > 1) && (argv[1][0] != '-')) + arg = std::string(argv[1]); + if (pcl::console::find_argument(argc, argv, "-h") == -1) { usage(argv); return 1; @@ -286,17 +290,17 @@ main(int argc, char** argv) float alpha = default_alpha; pcl::console::parse_argument(argc, argv, "-persistence_alpha", alpha); - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { PCL_INFO("PointXYZRGBA mode enabled.\n"); OpenNIFeaturePersistence v( - subsampling_leaf_size, normal_search_radius, scales_vector, alpha, ""); + subsampling_leaf_size, normal_search_radius, scales_vector, alpha, arg); v.run(); } else { PCL_INFO("PointXYZ mode enabled.\n"); OpenNIFeaturePersistence v( - subsampling_leaf_size, normal_search_radius, scales_vector, alpha, ""); + subsampling_leaf_size, normal_search_radius, scales_vector, alpha, arg); v.run(); } diff --git a/apps/src/openni_grab_images.cpp b/apps/src/openni_grab_images.cpp index 7dacf080..68503d9d 100644 --- a/apps/src/openni_grab_images.cpp +++ b/apps/src/openni_grab_images.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include @@ -378,12 +377,9 @@ main(int argc, char** argv) << "Supported image modes for device: " << device->getVendorName() << " , " << device->getProductName() << std::endl; modes = grabber.getAvailableImageModes(); - for (std::vector>::const_iterator it = - modes.begin(); - it != modes.end(); - ++it) { - std::cout << it->first << " = " << it->second.nXRes << " x " - << it->second.nYRes << " @ " << it->second.nFPS << std::endl; + for (const auto& mode : modes) { + std::cout << mode.first << " = " << mode.second.nXRes << " x " + << mode.second.nYRes << " @ " << mode.second.nFPS << std::endl; } } } diff --git a/apps/src/openni_ii_normal_estimation.cpp b/apps/src/openni_ii_normal_estimation.cpp index 1644b26b..ea918916 100644 --- a/apps/src/openni_ii_normal_estimation.cpp +++ b/apps/src/openni_ii_normal_estimation.cpp @@ -158,7 +158,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb(cloud); @@ -230,15 +230,15 @@ main(int argc, char** argv) std::cout << " quit\n\n"; // clang-format on - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { PCL_INFO("PointXYZRGBA mode enabled.\n"); - OpenNIIntegralImageNormalEstimation v(""); + OpenNIIntegralImageNormalEstimation v(arg); v.run(); } else { PCL_INFO("PointXYZ mode enabled.\n"); - OpenNIIntegralImageNormalEstimation v(""); + OpenNIIntegralImageNormalEstimation v(arg); v.run(); } diff --git a/apps/src/openni_klt.cpp b/apps/src/openni_klt.cpp index b553ef59..6bab5e4e 100644 --- a/apps/src/openni_klt.cpp +++ b/apps/src/openni_klt.cpp @@ -326,12 +326,9 @@ main(int argc, char** argv) << " , " << device->getProductName() << std::endl; std::vector> modes = grabber.getAvailableDepthModes(); - for (std::vector>::const_iterator it = - modes.begin(); - it != modes.end(); - ++it) { - std::cout << it->first << " = " << it->second.nXRes << " x " - << it->second.nYRes << " @ " << it->second.nFPS << std::endl; + for (const auto& mode : modes) { + std::cout << mode.first << " = " << mode.second.nXRes << " x " + << mode.second.nYRes << " @ " << mode.second.nFPS << std::endl; } if (device->hasImageStream()) { @@ -339,12 +336,9 @@ main(int argc, char** argv) << "Supported image modes for device: " << device->getVendorName() << " , " << device->getProductName() << std::endl; modes = grabber.getAvailableImageModes(); - for (std::vector>::const_iterator it = - modes.begin(); - it != modes.end(); - ++it) { - std::cout << it->first << " = " << it->second.nXRes << " x " - << it->second.nYRes << " @ " << it->second.nFPS << std::endl; + for (const auto& mode : modes) { + std::cout << mode.first << " = " << mode.second.nXRes << " x " + << mode.second.nYRes << " @ " << mode.second.nFPS << std::endl; } } } diff --git a/apps/src/openni_mls_smoothing.cpp b/apps/src/openni_mls_smoothing.cpp index c451e11b..c4551432 100644 --- a/apps/src/openni_mls_smoothing.cpp +++ b/apps/src/openni_mls_smoothing.cpp @@ -134,7 +134,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb_(cloud); diff --git a/apps/src/openni_mobile_server.cpp b/apps/src/openni_mobile_server.cpp index b663d91c..5a1c8d62 100644 --- a/apps/src/openni_mobile_server.cpp +++ b/apps/src/openni_mobile_server.cpp @@ -221,7 +221,7 @@ public: void usage(char** argv) { - std::cout << "usage: " << argv[0] << " \n" + std::cout << "usage: " << argv[0] << " \n" << "where options are:\n" << " -port p :: set the server port (default: 11111)\n" << " -leaf x, y, z :: set the voxel grid leaf size (default: 0.01)\n"; @@ -230,6 +230,10 @@ usage(char** argv) int main(int argc, char** argv) { + std::string device_id = ""; + if ((argc > 1) && (argv[1][0] != '-')) + device_id = std::string(argv[1]); + if (pcl::console::find_argument(argc, argv, "-h") != -1) { usage(argv); return 0; @@ -237,12 +241,11 @@ main(int argc, char** argv) int port = 11111; float leaf_x = 0.01f, leaf_y = 0.01f, leaf_z = 0.01f; - std::string device_id; pcl::console::parse_argument(argc, argv, "-port", port); pcl::console::parse_3x_arguments(argc, argv, "-leaf", leaf_x, leaf_y, leaf_z, false); - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(device_id); if (!grabber.providesCallback()) { std::cout << "OpenNI grabber does not provide the rgba cloud format." << std::endl; return 1; diff --git a/apps/src/openni_octree_compression.cpp b/apps/src/openni_octree_compression.cpp index 7203f1b2..0a056054 100644 --- a/apps/src/openni_octree_compression.cpp +++ b/apps/src/openni_octree_compression.cpp @@ -149,7 +149,7 @@ public: { // create a new grabber for OpenNI devices - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; // make callback function from member function std::function::ConstPtr&)> f = @@ -206,7 +206,7 @@ struct EventHelper { run() { // create a new grabber for OpenNI devices - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; // make callback function from member function std::function::ConstPtr&)> f = diff --git a/apps/src/openni_organized_compression.cpp b/apps/src/openni_organized_compression.cpp index ae855521..c0448bde 100644 --- a/apps/src/openni_organized_compression.cpp +++ b/apps/src/openni_organized_compression.cpp @@ -147,7 +147,7 @@ public: if (!bRawImageEncoding_) { // create a new grabber for OpenNI devices - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; // make callback function from member function std::function::ConstPtr&)> f = @@ -253,7 +253,7 @@ struct EventHelper { { if (!bRawImageEncoding_) { // create a new grabber for OpenNI devices - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; // make callback function from member function std::function::ConstPtr&)> f = diff --git a/apps/src/openni_organized_edge_detection.cpp b/apps/src/openni_organized_edge_detection.cpp index 2f4d07cb..caf01aae 100644 --- a/apps/src/openni_organized_edge_detection.cpp +++ b/apps/src/openni_organized_edge_detection.cpp @@ -53,7 +53,7 @@ public: OpenNIOrganizedEdgeDetection() : viewer(new pcl::visualization::PCLVisualizer("PCL Organized Edge Detection")) {} - ~OpenNIOrganizedEdgeDetection() {} + ~OpenNIOrganizedEdgeDetection() = default; pcl::visualization::PCLVisualizer::Ptr initCloudViewer(const pcl::PointCloud::ConstPtr& cloud) @@ -175,7 +175,7 @@ public: void run() { - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; std::function::ConstPtr&)> f = [this](const pcl::PointCloud::ConstPtr& cloud) { cloud_cb_(cloud); }; @@ -325,7 +325,7 @@ main(int argc, char** argv) //std::cout << "<5> EDGELABEL_RGB_CANNY edge" << std::endl; std::cout << " quit" << std::endl; // clang-format on - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { OpenNIOrganizedEdgeDetection app; app.run(); diff --git a/apps/src/openni_organized_multi_plane_segmentation.cpp b/apps/src/openni_organized_multi_plane_segmentation.cpp index 91030eab..a587a7e2 100644 --- a/apps/src/openni_organized_multi_plane_segmentation.cpp +++ b/apps/src/openni_organized_multi_plane_segmentation.cpp @@ -53,8 +53,8 @@ private: std::mutex cloud_mutex; public: - OpenNIOrganizedMultiPlaneSegmentation() {} - ~OpenNIOrganizedMultiPlaneSegmentation() {} + OpenNIOrganizedMultiPlaneSegmentation() = default; + ~OpenNIOrganizedMultiPlaneSegmentation() = default; pcl::visualization::PCLVisualizer::Ptr cloudViewer(const pcl::PointCloud::ConstPtr& cloud) @@ -100,7 +100,7 @@ public: void run() { - pcl::OpenNIGrabber interface{}; + pcl::OpenNIGrabber interface; std::function::ConstPtr&)> f = [this](const pcl::PointCloud::ConstPtr& cloud) { cloud_cb_(cloud); }; diff --git a/apps/src/openni_planar_convex_hull.cpp b/apps/src/openni_planar_convex_hull.cpp index ba4deeae..0cc1223f 100644 --- a/apps/src/openni_planar_convex_hull.cpp +++ b/apps/src/openni_planar_convex_hull.cpp @@ -117,7 +117,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb_(cloud); diff --git a/apps/src/openni_planar_segmentation.cpp b/apps/src/openni_planar_segmentation.cpp index 7256510f..cb9afd70 100644 --- a/apps/src/openni_planar_segmentation.cpp +++ b/apps/src/openni_planar_segmentation.cpp @@ -111,7 +111,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb_(cloud); diff --git a/apps/src/openni_tracking.cpp b/apps/src/openni_tracking.cpp index ccc6377a..1ea742c6 100644 --- a/apps/src/openni_tracking.cpp +++ b/apps/src/openni_tracking.cpp @@ -601,7 +601,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb(cloud); }; diff --git a/apps/src/openni_uniform_sampling.cpp b/apps/src/openni_uniform_sampling.cpp index 52a56fbc..fbce9edc 100644 --- a/apps/src/openni_uniform_sampling.cpp +++ b/apps/src/openni_uniform_sampling.cpp @@ -118,7 +118,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb_(cloud); diff --git a/apps/src/openni_voxel_grid.cpp b/apps/src/openni_voxel_grid.cpp index b51395fc..91274e73 100644 --- a/apps/src/openni_voxel_grid.cpp +++ b/apps/src/openni_voxel_grid.cpp @@ -111,7 +111,7 @@ public: void run() { - pcl::OpenNIGrabber interface{device_id_}; + pcl::OpenNIGrabber interface(device_id_); std::function f = [this](const CloudConstPtr& cloud) { cloud_cb_(cloud); @@ -169,8 +169,14 @@ usage(char** argv) int main(int argc, char** argv) { - if (pcl::console::find_argument(argc, argv, "-h") != -1) + std::string arg = ""; + if ((argc > 1) && (argv[1][0] != '-')) + arg = std::string(argv[1]); + + if (pcl::console::find_argument(argc, argv, "-h") != -1) { usage(argv); + return 1; + } float min_v = 0.0f, max_v = 5.0f; pcl::console::parse_2x_arguments(argc, argv, "-minmax", min_v, max_v); @@ -182,15 +188,15 @@ main(int argc, char** argv) pcl::console::parse_3x_arguments(argc, argv, "-leaf", leaf_x, leaf_y, leaf_z); PCL_INFO("Using %f, %f, %f as a leaf size for VoxelGrid.\n", leaf_x, leaf_y, leaf_z); - pcl::OpenNIGrabber grabber(""); + pcl::OpenNIGrabber grabber(arg); if (grabber.providesCallback()) { OpenNIVoxelGrid v( - "", field_name, min_v, max_v, leaf_x, leaf_y, leaf_z); + arg, field_name, min_v, max_v, leaf_x, leaf_y, leaf_z); v.run(); } else { OpenNIVoxelGrid v( - "", field_name, min_v, max_v, leaf_x, leaf_y, leaf_z); + arg, field_name, min_v, max_v, leaf_x, leaf_y, leaf_z); v.run(); } diff --git a/apps/src/pcd_video_player/pcd_video_player.cpp b/apps/src/pcd_video_player/pcd_video_player.cpp index 0e90e874..527a1872 100644 --- a/apps/src/pcd_video_player/pcd_video_player.cpp +++ b/apps/src/pcd_video_player/pcd_video_player.cpp @@ -47,6 +47,10 @@ #include #include #include +#if VTK_MAJOR_VERSION >= 9 || (VTK_MAJOR_VERSION == 8 && VTK_MINOR_VERSION >= 2) +#define HAS_QVTKOPENGLWINDOW_H +#include +#endif #include #include @@ -310,6 +314,9 @@ print_usage() int main(int argc, char** argv) { +#ifdef HAS_QVTKOPENGLWINDOW_H + QSurfaceFormat::setDefaultFormat(QVTKOpenGLWindow::defaultFormat()); +#endif QApplication app(argc, argv); PCDVideoPlayer VideoPlayer; diff --git a/apps/src/render_views_tesselated_sphere.cpp b/apps/src/render_views_tesselated_sphere.cpp index fd1a7a33..ac248ac0 100644 --- a/apps/src/render_views_tesselated_sphere.cpp +++ b/apps/src/render_views_tesselated_sphere.cpp @@ -94,21 +94,6 @@ pcl::apps::RenderViewsTesselatedSphere::generateViews() mapper->SetInputConnection(trans_filter_scale->GetOutputPort()); mapper->Update(); - ////////////////////////////// - // * Compute area of the mesh - ////////////////////////////// - vtkSmartPointer cells = mapper->GetInput()->GetPolys(); - vtkIdType npts = 0; - vtkCellPtsPtr ptIds = nullptr; - - double p1[3], p2[3], p3[3], totalArea = 0; - for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds);) { - polydata_->GetPoint(ptIds[0], p1); - polydata_->GetPoint(ptIds[1], p2); - polydata_->GetPoint(ptIds[2], p3); - totalArea += vtkTriangle::TriangleArea(p1, p2, p3); - } - // create icosahedron vtkSmartPointer ico = vtkSmartPointer::New(); diff --git a/apps/src/stereo_ground_segmentation.cpp b/apps/src/stereo_ground_segmentation.cpp index baabce97..0ef2ed02 100755 --- a/apps/src/stereo_ground_segmentation.cpp +++ b/apps/src/stereo_ground_segmentation.cpp @@ -160,7 +160,7 @@ public: road_comparator->setAngularThreshold(pcl::deg2rad(3.0f)); } - ~HRCSSegmentation() {} + ~HRCSSegmentation() = default; void cloud_cb_(const pcl::PointCloud::ConstPtr& cloud) diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index 6fccc23f..b85bc035 100644 --- a/benchmarks/CMakeLists.txt +++ b/benchmarks/CMakeLists.txt @@ -5,12 +5,14 @@ set(DEFAULT OFF) set(build TRUE) set(REASON "Disabled by default") PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) if(NOT build) return() endif() find_package(benchmark REQUIRED) +get_target_property(BenchmarkBuildType benchmark::benchmark TYPE) + add_custom_target(run_benchmarks) PCL_ADD_BENCHMARK(features_normal_3d FILES features/normal_3d.cpp diff --git a/cmake/Modules/UseCompilerCache.cmake b/cmake/Modules/UseCompilerCache.cmake index 0c21d296..06001ece 100644 --- a/cmake/Modules/UseCompilerCache.cmake +++ b/cmake/Modules/UseCompilerCache.cmake @@ -42,6 +42,10 @@ function(UseCompilerCache) set(multiValueArgs) cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to UseCompilerCache: ${ARGS_UNPARSED_ARGUMENTS}") + endif() if(NOT ARGS_CCACHE) set(ARGS_CCACHE ccache) @@ -74,7 +78,7 @@ function(UseCompilerCache) endif() endforeach() - if(NOT QUIET) + if(NOT ARGS_QUIET) message(STATUS "Using Compiler Cache (${CCACHE_PROGRAM}) v${version} in the C/C++ toolchain") endif() @@ -105,12 +109,7 @@ function(UseCompilerCache) "${CMAKE_BINARY_DIR}/launch-c" "${CMAKE_BINARY_DIR}/launch-cxx") - # Cuda support only added in CMake 3.10 - set(cuda_supported FALSE) - if (NOT (CMAKE_VERSION VERSION_LESS 3.10) AND CMAKE_CUDA_COMPILER) - set(cuda_supported TRUE) - endif() - if(${cuda_supported}) + if(CMAKE_CUDA_COMPILER) pcl_ccache_compat_file_gen("launch-cuda" ${CCACHE_PROGRAM} ${CMAKE_CUDA_COMPILER}) execute_process(COMMAND chmod a+rx "${CMAKE_BINARY_DIR}/launch-cuda") @@ -127,7 +126,7 @@ function(UseCompilerCache) message(STATUS "Compiler cache via launch files to support Unix Makefiles and Ninja") set(CMAKE_C_COMPILER_LAUNCHER "${CMAKE_BINARY_DIR}/launch-c" PARENT_SCOPE) set(CMAKE_CXX_COMPILER_LAUNCHER "${CMAKE_BINARY_DIR}/launch-cxx" PARENT_SCOPE) - if (${cuda_supported}) + if (CMAKE_CUDA_COMPILER) set(CMAKE_CUDA_COMPILER_LAUNCHER "${CMAKE_BINARY_DIR}/launch-cuda" PARENT_SCOPE) endif() endif() diff --git a/cmake/clang-format.cmake b/cmake/clang-format.cmake index 9d411404..fa32aba8 100644 --- a/cmake/clang-format.cmake +++ b/cmake/clang-format.cmake @@ -1,4 +1,4 @@ -find_package(ClangFormat 10) +find_package(ClangFormat 14) # search for version number in clang-format without version number if(ClangFormat_FOUND) message(STATUS "Adding target 'format'") diff --git a/cmake/pcl_find_boost.cmake b/cmake/pcl_find_boost.cmake index 862851b1..b60fafae 100644 --- a/cmake/pcl_find_boost.cmake +++ b/cmake/pcl_find_boost.cmake @@ -14,7 +14,8 @@ else() endif() set(Boost_ADDITIONAL_VERSIONS - "1.78.0" "1.78" "1.77.0" "1.77" "1.76.0" "1.76" "1.75.0" "1.75" + "1.80.0" "1.80" + "1.79.0" "1.79" "1.78.0" "1.78" "1.77.0" "1.77" "1.76.0" "1.76" "1.75.0" "1.75" "1.74.0" "1.74" "1.73.0" "1.73" "1.72.0" "1.72" "1.71.0" "1.71" "1.70.0" "1.70" "1.69.0" "1.69" "1.68.0" "1.68" "1.67.0" "1.67" "1.66.0" "1.66" "1.65.1" "1.65.0" "1.65") diff --git a/cmake/pcl_find_vtk.cmake b/cmake/pcl_find_vtk.cmake index 918d8873..ab48e742 100644 --- a/cmake/pcl_find_vtk.cmake +++ b/cmake/pcl_find_vtk.cmake @@ -1,9 +1,13 @@ function(checkVTKComponents) - cmake_parse_arguments(PARAM "" "MISSING_COMPONENTS" "COMPONENTS" ${ARGN}) + cmake_parse_arguments(ARGS "" "MISSING_COMPONENTS" "COMPONENTS" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to checkVTKComponents: ${ARGS_UNPARSED_ARGUMENTS}") + endif() set(vtkMissingComponents) - foreach(vtkComponent ${PARAM_COMPONENTS}) + foreach(vtkComponent ${ARGS_COMPONENTS}) if (VTK_VERSION VERSION_LESS 9.0) if (NOT TARGET ${vtkComponent}) list(APPEND vtkMissingComponents ${vtkComponent}) @@ -15,7 +19,9 @@ function(checkVTKComponents) endif() endforeach() - set(${PARAM_MISSING_COMPONENTS} ${vtkMissingComponents} PARENT_SCOPE) + if(ARGS_MISSING_COMPONENTS) + set(${ARGS_MISSING_COMPONENTS} ${vtkMissingComponents} PARENT_SCOPE) + endif() endfunction() # Start with a generic call to find any VTK version we are supporting, so we retrieve @@ -69,21 +75,9 @@ set(NON_PREFIX_PCL_VTK_COMPONENTS ViewsContext2D ) -#If VTK version 6 use OpenGL -if(VTK_VERSION VERSION_LESS 7.0) - set(VTK_RENDERING_BACKEND "OpenGL") - set(VTK_RENDERING_BACKEND_OPENGL_VERSION "1") - message(DEPRECATION "The rendering backend OpenGL is deprecated and not available anymore since VTK 8.2." - "Please switch to the OpenGL2 backend instead, which is available since VTK 6.2." - "Support of the deprecated backend will be dropped with PCL 1.13.") - -#If VTK version 7,8 or 9 use OpenGL2 -else() - set(VTK_RENDERING_BACKEND "OpenGL2") - set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2") -endif() - -list(APPEND NON_PREFIX_PCL_VTK_COMPONENTS Rendering${VTK_RENDERING_BACKEND}) +set(VTK_RENDERING_BACKEND "OpenGL2") +set(VTK_RENDERING_BACKEND_OPENGL_VERSION "2") +list(APPEND NON_PREFIX_PCL_VTK_COMPONENTS Rendering${VTK_RENDERING_BACKEND} RenderingContext${VTK_RENDERING_BACKEND}) #Append vtk to components if version is <9.0 if(VTK_VERSION VERSION_LESS 9.0) @@ -99,8 +93,7 @@ endif() checkVTKComponents(COMPONENTS ${PCL_VTK_COMPONENTS} MISSING_COMPONENTS vtkMissingComponents) if (vtkMissingComponents) - set(VTK_FOUND FALSE) - message(WARNING "Missing vtk modules: ${vtkMissingComponents}") + message(FATAL_ERROR "Missing vtk modules: ${vtkMissingComponents}") endif() if("vtkGUISupportQt" IN_LIST VTK_MODULES_ENABLED) diff --git a/cmake/pcl_targets.cmake b/cmake/pcl_targets.cmake index ff79c207..86d06d0c 100644 --- a/cmake/pcl_targets.cmake +++ b/cmake/pcl_targets.cmake @@ -81,26 +81,39 @@ endmacro() # they are not being built. # _var The cumulative build variable. This will be set to FALSE if the # dependencies are not met. -# _name The name of the subsystem. # ARGN The subsystems and external libraries to depend on. -macro(PCL_SUBSYS_DEPEND _var _name) +macro(PCL_SUBSYS_DEPEND _var) set(options) set(oneValueArgs) - set(multiValueArgs DEPS EXT_DEPS OPT_DEPS) - cmake_parse_arguments(SUBSYS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) - if(SUBSYS_DEPS) - SET_IN_GLOBAL_MAP(PCL_SUBSYS_DEPS ${_name} "${SUBSYS_DEPS}") + set(multiValueArgs DEPS EXT_DEPS OPT_DEPS NAME PARENT_NAME) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_SUBSYS_DEPEND: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARGS_NAME) + message(FATAL_ERROR "PCL_SUBSYS_DEPEND requires parameter NAME!") endif() - if(SUBSYS_EXT_DEPS) - SET_IN_GLOBAL_MAP(PCL_SUBSYS_EXT_DEPS ${_name} "${SUBSYS_EXT_DEPS}") + + set(_name ${ARGS_NAME}) + if(ARGS_PARENT_NAME) + string(PREPEND _name "${ARGS_PARENT_NAME}_") + endif() + + if(ARGS_DEPS) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_DEPS ${_name} "${ARGS_DEPS}") endif() - if(SUBSYS_OPT_DEPS) - SET_IN_GLOBAL_MAP(PCL_SUBSYS_OPT_DEPS ${_name} "${SUBSYS_OPT_DEPS}") + if(ARGS_EXT_DEPS) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_EXT_DEPS ${_name} "${ARGS_EXT_DEPS}") + endif() + if(ARGS_OPT_DEPS) + SET_IN_GLOBAL_MAP(PCL_SUBSYS_OPT_DEPS ${_name} "${ARGS_OPT_DEPS}") endif() GET_IN_MAP(subsys_status PCL_SUBSYS_HYPERSTATUS ${_name}) if(${_var} AND (NOT ("${subsys_status}" STREQUAL "AUTO_OFF"))) - if(SUBSYS_DEPS) - foreach(_dep ${SUBSYS_DEPS}) + if(ARGS_DEPS) + foreach(_dep ${ARGS_DEPS}) PCL_GET_SUBSYS_STATUS(_status ${_dep}) if(NOT _status) set(${_var} FALSE) @@ -111,8 +124,8 @@ macro(PCL_SUBSYS_DEPEND _var _name) endif() endforeach() endif() - if(SUBSYS_EXT_DEPS) - foreach(_dep ${SUBSYS_EXT_DEPS}) + if(ARGS_EXT_DEPS) + foreach(_dep ${ARGS_EXT_DEPS}) string(TOUPPER "${_dep}_found" EXT_DEP_FOUND) #Variable EXT_DEP_FOUND expands to ie. QHULL_FOUND which in turn is then used to see if the EXT_DEPS is found. if(NOT ${EXT_DEP_FOUND}) @@ -121,8 +134,8 @@ macro(PCL_SUBSYS_DEPEND _var _name) endif() endforeach() endif() - if(SUBSYS_OPT_DEPS) - foreach(_dep ${SUBSYS_OPT_DEPS}) + if(ARGS_OPT_DEPS) + foreach(_dep ${ARGS_OPT_DEPS}) PCL_GET_SUBSYS_INCLUDE_DIR(_include_dir ${_dep}) include_directories(${PROJECT_SOURCE_DIR}/${_include_dir}/include) endforeach() @@ -130,55 +143,6 @@ macro(PCL_SUBSYS_DEPEND _var _name) endif() endmacro() -############################################################################### -# Make one subsystem depend on one or more other subsystems, and disable it if -# they are not being built. -# _var The cumulative build variable. This will be set to FALSE if the -# dependencies are not met. -# _parent The parent subsystem name. -# _name The name of the subsubsystem. -# ARGN The subsystems and external libraries to depend on. -macro(PCL_SUBSUBSYS_DEPEND _var _parent _name) - set(options) - set(parentArg) - set(nameArg) - set(multiValueArgs DEPS EXT_DEPS OPT_DEPS) - cmake_parse_arguments(SUBSYS "${options}" "${parentArg}" "${nameArg}" "${multiValueArgs}" ${ARGN}) - if(SUBSUBSYS_DEPS) - SET_IN_GLOBAL_MAP(PCL_SUBSYS_DEPS ${_parent}_${_name} "${SUBSUBSYS_DEPS}") - endif() - if(SUBSUBSYS_EXT_DEPS) - SET_IN_GLOBAL_MAP(PCL_SUBSYS_EXT_DEPS ${_parent}_${_name} "${SUBSUBSYS_EXT_DEPS}") - endif() - if(SUBSUBSYS_OPT_DEPS) - SET_IN_GLOBAL_MAP(PCL_SUBSYS_OPT_DEPS ${_parent}_${_name} "${SUBSUBSYS_OPT_DEPS}") - endif() - GET_IN_MAP(subsys_status PCL_SUBSYS_HYPERSTATUS ${_parent}_${_name}) - if(${_var} AND (NOT ("${subsys_status}" STREQUAL "AUTO_OFF"))) - if(SUBSUBSYS_DEPS) - foreach(_dep ${SUBSUBSYS_DEPS}) - PCL_GET_SUBSYS_STATUS(_status ${_dep}) - if(NOT _status) - set(${_var} FALSE) - PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE "Requires ${_dep}.") - else() - PCL_GET_SUBSYS_INCLUDE_DIR(_include_dir ${_dep}) - include_directories(${PROJECT_SOURCE_DIR}/${_include_dir}/include) - endif() - endforeach() - endif() - if(SUBSUBSYS_EXT_DEPS) - foreach(_dep ${SUBSUBSYS_EXT_DEPS}) - string(TOUPPER "${_dep}_found" EXT_DEP_FOUND) - if(NOT ${EXT_DEP_FOUND}) - set(${_var} FALSE) - PCL_SET_SUBSYS_STATUS(${_parent}_${_name} FALSE "Requires external library ${_dep}.") - endif() - endforeach() - endif() - endif() -endmacro() - ############################################################################### # Adds version information to executable/library in form of a version.rc. This works only with MSVC. # @@ -218,9 +182,17 @@ function(PCL_ADD_LIBRARY _name) set(options) set(oneValueArgs COMPONENT) set(multiValueArgs SOURCES) - cmake_parse_arguments(ADD_LIBRARY_OPTION "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_ADD_LIBRARY: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARGS_COMPONENT) + message(FATAL_ERROR "PCL_ADD_LIBRARY requires parameter COMPONENT.") + endif() - add_library(${_name} ${PCL_LIB_TYPE} ${ADD_LIBRARY_OPTION_SOURCES}) + add_library(${_name} ${PCL_LIB_TYPE} ${ARGS_SOURCES}) PCL_ADD_VERSION_INFO(${_name}) target_compile_features(${_name} PUBLIC ${PCL_CXX_COMPILE_FEATURES}) @@ -230,7 +202,7 @@ function(PCL_ADD_LIBRARY _name) endif() if((UNIX AND NOT ANDROID) OR MINGW) - target_link_libraries(${_name} m) + target_link_libraries(${_name} m ${ATOMIC_LIBRARY}) endif() if(MINGW) @@ -248,12 +220,12 @@ function(PCL_ADD_LIBRARY _name) set_target_properties(${_name} PROPERTIES FOLDER "Libraries") install(TARGETS ${_name} - RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT} - LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT} - ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT}) + RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT} + LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT} + ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT}) # Copy PDB if available - if(MSVC AND PCL_SHARED_LIBS) + if(MSVC AND ${PCL_LIB_TYPE} EQUAL "SHARED") install(FILES $ DESTINATION ${BIN_INSTALL_DIR} OPTIONAL) endif() endfunction() @@ -267,11 +239,19 @@ function(PCL_CUDA_ADD_LIBRARY _name) set(options) set(oneValueArgs COMPONENT) set(multiValueArgs SOURCES) - cmake_parse_arguments(ADD_LIBRARY_OPTION "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_CUDA_ADD_LIBRARY: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARGS_COMPONENT) + message(FATAL_ERROR "PCL_CUDA_ADD_LIBRARY requires parameter COMPONENT.") + endif() REMOVE_VTK_DEFINITIONS() - add_library(${_name} ${PCL_LIB_TYPE} ${ADD_LIBRARY_OPTION_SOURCES}) + add_library(${_name} ${PCL_LIB_TYPE} ${ARGS_SOURCES}) PCL_ADD_VERSION_INFO(${_name}) @@ -279,16 +259,20 @@ function(PCL_CUDA_ADD_LIBRARY _name) target_include_directories(${_name} PRIVATE ${CUDA_TOOLKIT_INCLUDE}) + if(MSVC) + target_link_libraries(${_name} delayimp.lib) # because delay load is enabled for openmp.dll + endif() + set_target_properties(${_name} PROPERTIES VERSION ${PCL_VERSION} - SOVERSION ${PCL_VERSION_MAJOR} + SOVERSION ${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR} DEFINE_SYMBOL "PCLAPI_EXPORTS") set_target_properties(${_name} PROPERTIES FOLDER "Libraries") install(TARGETS ${_name} - RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT} - LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT} - ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT}) + RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT} + LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT} + ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT}) endfunction() ############################################################################### @@ -301,15 +285,23 @@ function(PCL_ADD_EXECUTABLE _name) set(options BUNDLE) set(oneValueArgs COMPONENT) set(multiValueArgs SOURCES) - cmake_parse_arguments(ADD_LIBRARY_OPTION "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_ADD_EXECUTABLE: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARGS_COMPONENT) + message(FATAL_ERROR "PCL_ADD_EXECUTABLE requires parameter COMPONENT.") + endif() - if(ADD_LIBRARY_OPTION_BUNDLE AND APPLE AND VTK_USE_COCOA) - add_executable(${_name} MACOSX_BUNDLE ${ADD_LIBRARY_OPTION_SOURCES}) + if(ARGS_BUNDLE AND APPLE AND VTK_USE_COCOA) + add_executable(${_name} MACOSX_BUNDLE ${ARGS_SOURCES}) else() - add_executable(${_name} ${ADD_LIBRARY_OPTION_SOURCES}) + add_executable(${_name} ${ARGS_SOURCES}) endif() PCL_ADD_VERSION_INFO(${_name}) - + target_link_libraries(${_name} Threads::Threads) if(WIN32 AND MSVC) @@ -319,7 +311,7 @@ function(PCL_ADD_EXECUTABLE _name) # Some app targets report are defined with subsys other than apps # It's simpler check for tools and assume everythin else as an app - if(${ADD_LIBRARY_OPTION_COMPONENT} STREQUAL "tools") + if(${ARGS_COMPONENT} STREQUAL "tools") set_target_properties(${_name} PROPERTIES FOLDER "Tools") else() set_target_properties(${_name} PROPERTIES FOLDER "Apps") @@ -327,13 +319,13 @@ function(PCL_ADD_EXECUTABLE _name) set(PCL_EXECUTABLES ${PCL_EXECUTABLES} ${_name}) - if(ADD_LIBRARY_OPTION_BUNDLE AND APPLE AND VTK_USE_COCOA) - install(TARGETS ${_name} BUNDLE DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT}) + if(ARGS_BUNDLE AND APPLE AND VTK_USE_COCOA) + install(TARGETS ${_name} BUNDLE DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT}) else() - install(TARGETS ${_name} RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT}) + install(TARGETS ${_name} RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT pcl_${ARGS_COMPONENT}) endif() - string(TOUPPER ${ADD_LIBRARY_OPTION_COMPONENT} _component_upper) + string(TOUPPER ${ARGS_COMPONENT} _component_upper) set(PCL_${_component_upper}_ALL_TARGETS ${PCL_${_component_upper}_ALL_TARGETS} ${_name} PARENT_SCOPE) endfunction() @@ -346,16 +338,24 @@ function(PCL_CUDA_ADD_EXECUTABLE _name) set(options) set(oneValueArgs COMPONENT) set(multiValueArgs SOURCES) - cmake_parse_arguments(ADD_LIBRARY_OPTION "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_CUDA_ADD_EXECUTABLE: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARGS_COMPONENT) + message(FATAL_ERROR "PCL_CUDA_ADD_EXECUTABLE requires parameter COMPONENT.") + endif() REMOVE_VTK_DEFINITIONS() - - add_executable(${_name} ${ADD_LIBRARY_OPTION_SOURCES}) - + + add_executable(${_name} ${ARGS_SOURCES}) + PCL_ADD_VERSION_INFO(${_name}) target_compile_options(${_name} PRIVATE $<$: ${GEN_CODE} --expt-relaxed-constexpr>) - + target_include_directories(${_name} PRIVATE ${CUDA_TOOLKIT_INCLUDE}) if(WIN32 AND MSVC) @@ -368,7 +368,7 @@ function(PCL_CUDA_ADD_EXECUTABLE _name) set(PCL_EXECUTABLES ${PCL_EXECUTABLES} ${_name}) install(TARGETS ${_name} RUNTIME DESTINATION ${BIN_INSTALL_DIR} - COMPONENT pcl_${ADD_LIBRARY_OPTION_COMPONENT}) + COMPONENT pcl_${ARGS_COMPONENT}) endfunction() ############################################################################### @@ -383,15 +383,20 @@ macro(PCL_ADD_TEST _name _exename) set(options) set(oneValueArgs) set(multiValueArgs FILES ARGUMENTS LINK_WITH) - cmake_parse_arguments(PCL_ADD_TEST "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) - add_executable(${_exename} ${PCL_ADD_TEST_FILES}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_ADD_TEST: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + add_executable(${_exename} ${ARGS_FILES}) if(NOT WIN32) set_target_properties(${_exename} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) endif() - #target_link_libraries(${_exename} ${GTEST_BOTH_LIBRARIES} ${PCL_ADD_TEST_LINK_WITH}) - target_link_libraries(${_exename} ${PCL_ADD_TEST_LINK_WITH} ${CLANG_LIBRARIES}) + #target_link_libraries(${_exename} ${GTEST_BOTH_LIBRARIES} ${ARGS_LINK_WITH}) + target_link_libraries(${_exename} ${ARGS_LINK_WITH} ${CLANG_LIBRARIES}) - target_link_libraries(${_exename} Threads::Threads) + target_link_libraries(${_exename} Threads::Threads ${ATOMIC_LIBRARY}) #Only applies to MSVC if(MSVC) @@ -401,15 +406,15 @@ macro(PCL_ADD_TEST _name _exename) SET (ArgumentWarningShown TRUE PARENT_SCOPE) else() #Only add if there are arguments to test - if(PCL_ADD_TEST_ARGUMENTS) - string (REPLACE ";" " " PCL_ADD_TEST_ARGUMENTS_STR "${PCL_ADD_TEST_ARGUMENTS}") - set_target_properties(${_exename} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${PCL_ADD_TEST_ARGUMENTS_STR}) + if(ARGS_ARGUMENTS) + string (REPLACE ";" " " ARGS_ARGUMENTS_STR "${ARGS_ARGUMENTS}") + set_target_properties(${_exename} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${ARGS_ARGUMENTS_STR}) endif() endif() endif() set_target_properties(${_exename} PROPERTIES FOLDER "Tests") - add_test(NAME ${_name} COMMAND ${_exename} ${PCL_ADD_TEST_ARGUMENTS}) + add_test(NAME ${_name} COMMAND ${_exename} ${ARGS_ARGUMENTS}) add_dependencies(tests ${_exename}) endmacro() @@ -425,13 +430,22 @@ function(PCL_ADD_BENCHMARK _name) set(options) set(oneValueArgs) set(multiValueArgs FILES ARGUMENTS LINK_WITH) - cmake_parse_arguments(PCL_ADD_BENCHMARK "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_ADD_BENCHMARK: ${ARGS_UNPARSED_ARGUMENTS}") + endif() - add_executable(benchmark_${_name} ${PCL_ADD_BENCHMARK_FILES}) + add_executable(benchmark_${_name} ${ARGS_FILES}) set_target_properties(benchmark_${_name} PROPERTIES FOLDER "Benchmarks") - target_link_libraries(benchmark_${_name} benchmark::benchmark ${PCL_ADD_BENCHMARK_LINK_WITH}) + target_link_libraries(benchmark_${_name} benchmark::benchmark ${ARGS_LINK_WITH}) set_target_properties(benchmark_${_name} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) - + + # See https://github.com/google/benchmark/issues/1457 + if(BenchmarkBuildType STREQUAL "STATIC_LIBRARY" AND benchmark_VERSION STREQUAL "1.7.0") + target_compile_definitions(benchmark_${_name} PUBLIC -DBENCHMARK_STATIC_DEFINE) + endif() + #Only applies to MSVC if(MSVC) #Requires CMAKE version 3.13.0 @@ -441,16 +455,16 @@ function(PCL_ADD_BENCHMARK _name) set_target_properties(run_benchmarks PROPERTIES PCL_BENCHMARK_ARGUMENTS_WARNING_SHOWN TRUE) else() #Only add if there are arguments to test - if(PCL_ADD_BENCHMARK_ARGUMENTS) - string (REPLACE ";" " " PCL_ADD_BENCHMARK_ARGUMENTS_STR "${PCL_ADD_BENCHMARK_ARGUMENTS}") - set_target_properties(benchmark_${_name} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${PCL_ADD_BENCHMARK_ARGUMENTS_STR}) + if(ARGS_ARGUMENTS) + string (REPLACE ";" " " ARGS_ARGUMENTS_STR "${ARGS_ARGUMENTS}") + set_target_properties(benchmark_${_name} PROPERTIES VS_DEBUGGER_COMMAND_ARGUMENTS ${ARGS_ARGUMENTS_STR}) endif() endif() endif() - - add_custom_target(run_benchmark_${_name} benchmark_${_name} ${PCL_ADD_BENCHMARK_ARGUMENTS}) + + add_custom_target(run_benchmark_${_name} benchmark_${_name} ${ARGS_ARGUMENTS}) set_target_properties(run_benchmark_${_name} PROPERTIES FOLDER "Benchmarks") - + add_dependencies(run_benchmarks run_benchmark_${_name}) endfunction() @@ -464,9 +478,14 @@ macro(PCL_ADD_EXAMPLE _name) set(options) set(oneValueArgs) set(multiValueArgs FILES LINK_WITH) - cmake_parse_arguments(PCL_ADD_EXAMPLE "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) - add_executable(${_name} ${PCL_ADD_EXAMPLE_FILES}) - target_link_libraries(${_name} ${PCL_ADD_EXAMPLE_LINK_WITH} ${CLANG_LIBRARIES}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_ADD_EXAMPLE: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + add_executable(${_name} ${ARGS_FILES}) + target_link_libraries(${_name} ${ARGS_LINK_WITH} ${CLANG_LIBRARIES}) if(WIN32 AND MSVC) set_target_properties(${_name} PROPERTIES DEBUG_OUTPUT_NAME ${_name}${CMAKE_DEBUG_POSTFIX} RELEASE_OUTPUT_NAME ${_name}${CMAKE_RELEASE_POSTFIX}) @@ -522,30 +541,38 @@ function(PCL_MAKE_PKGCONFIG _name) set(options HEADER_ONLY) set(oneValueArgs COMPONENT DESC CFLAGS LIB_FLAGS) set(multiValueArgs PCL_DEPS INT_DEPS EXT_DEPS) - cmake_parse_arguments(PKGCONFIG "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + cmake_parse_arguments(ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) + + if(ARGS_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "Unknown arguments given to PCL_MAKE_PKGCONFIG: ${ARGS_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARGS_COMPONENT) + message(FATAL_ERROR "PCL_MAKE_PKGCONFIG requires parameter COMPONENT.") + endif() set(PKG_NAME ${_name}) - set(PKG_DESC ${PKGCONFIG_DESC}) - set(PKG_CFLAGS ${PKGCONFIG_CFLAGS}) - set(PKG_LIBFLAGS ${PKGCONFIG_LIB_FLAGS}) - LIST_TO_STRING(PKG_EXTERNAL_DEPS "${PKGCONFIG_EXT_DEPS}") - foreach(_dep ${PKGCONFIG_PCL_DEPS}) - string(APPEND PKG_EXTERNAL_DEPS " pcl_${_dep}-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}") + set(PKG_DESC ${ARGS_DESC}) + set(PKG_CFLAGS ${ARGS_CFLAGS}) + set(PKG_LIBFLAGS ${ARGS_LIB_FLAGS}) + LIST_TO_STRING(PKG_EXTERNAL_DEPS "${ARGS_EXT_DEPS}") + foreach(_dep ${ARGS_PCL_DEPS}) + string(APPEND PKG_EXTERNAL_DEPS " pcl_${_dep}") endforeach() set(PKG_INTERNAL_DEPS "") - foreach(_dep ${PKGCONFIG_INT_DEPS}) + foreach(_dep ${ARGS_INT_DEPS}) string(APPEND PKG_INTERNAL_DEPS " -l${_dep}") endforeach() - set(_pc_file ${CMAKE_CURRENT_BINARY_DIR}/${_name}-${PCL_VERSION_MAJOR}.${PCL_VERSION_MINOR}.pc) - if(PKGCONFIG_HEADER_ONLY) + set(_pc_file ${CMAKE_CURRENT_BINARY_DIR}/${_name}.pc) + if(ARGS_HEADER_ONLY) configure_file(${PROJECT_SOURCE_DIR}/cmake/pkgconfig-headeronly.cmake.in ${_pc_file} @ONLY) else() configure_file(${PROJECT_SOURCE_DIR}/cmake/pkgconfig.cmake.in ${_pc_file} @ONLY) endif() install(FILES ${_pc_file} DESTINATION ${PKGCFG_INSTALL_DIR} - COMPONENT pcl_${PKGCONFIG_COMPONENT}) + COMPONENT pcl_${ARGS_COMPONENT}) endfunction() ############################################################################### @@ -896,3 +923,18 @@ endmacro() macro(PCL_SET_TEST_DEPENDENCIES _var _module) set(${_var} global_tests ${_module} ${PCL_SUBSYS_DEPS_${_module}}) endmacro() + +############################################################################### +# Add two test targets for both values of PCL_RUN_TESTS_AT_COMPILE_TIME +# boolean flag, binaries produced are named with "_runtime" and "_compiletime" +# for false and true values accordingly. +# _name The test name. +# _exename The exe name. +# ARGN : +# see PCL_ADD_TEST documentation +macro (PCL_ADD_COMPILETIME_AND_RUNTIME_TEST _name _exename) + PCL_ADD_TEST("${_name}_runtime" "${_exename}_runtime" ${ARGN}) + target_compile_definitions("${_exename}_runtime" PRIVATE PCL_RUN_TESTS_AT_COMPILE_TIME=false) + PCL_ADD_TEST("${_name}_compiletime" "${_exename}_compiletime" ${ARGN}) + target_compile_definitions("${_exename}_compiletime" PRIVATE PCL_RUN_TESTS_AT_COMPILE_TIME=true) +endmacro() diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 2ef82d37..9c706733 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -4,7 +4,7 @@ set(SUBSYS_DEPS) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS eigen boost) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS eigen boost) PCL_ADD_DOC("${SUBSYS_NAME}") @@ -184,7 +184,8 @@ if(MSVC AND NOT (MSVC_VERSION LESS 1915)) target_compile_definitions(${LIB_NAME} PUBLIC _ENABLE_EXTENDED_ALIGNED_STORAGE) endif() -PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC}) +set(EXT_DEPS eigen3) +PCL_MAKE_PKGCONFIG(${LIB_NAME} COMPONENT ${SUBSYS_NAME} DESC ${SUBSYS_DESC} EXT_DEPS ${EXT_DEPS}) # Install include files PCL_ADD_INCLUDES("${SUBSYS_NAME}" "" ${incs}) diff --git a/common/include/pcl/ModelCoefficients.h b/common/include/pcl/ModelCoefficients.h index 84f7336b..3d1a5d0e 100644 --- a/common/include/pcl/ModelCoefficients.h +++ b/common/include/pcl/ModelCoefficients.h @@ -10,9 +10,7 @@ namespace pcl { struct ModelCoefficients { - ModelCoefficients () - { - } + ModelCoefficients () = default; ::pcl::PCLHeader header; diff --git a/common/include/pcl/PCLPointCloud2.h b/common/include/pcl/PCLPointCloud2.h index 50520f66..cf78ad39 100644 --- a/common/include/pcl/PCLPointCloud2.h +++ b/common/include/pcl/PCLPointCloud2.h @@ -85,6 +85,34 @@ namespace pcl { return (PCLPointCloud2 (*this) += rhs); } + + /** \brief Get value at specified offset. + * \param[in] point_index point index. + * \param[in] field_offset offset. + * \return value at the given offset. + */ + template inline + const T& at(const pcl::uindex_t& point_index, const pcl::uindex_t& field_offset) const { + const auto position = point_index * point_step + field_offset; + if (data.size () >= (position + sizeof(T))) + return reinterpret_cast(data[position]); + else + throw std::out_of_range("PCLPointCloud2::at"); + } + + /** \brief Get value at specified offset. + * \param[in] point_index point index. + * \param[in] field_offset offset. + * \return value at the given offset. + */ + template inline + T& at(const pcl::uindex_t& point_index, const pcl::uindex_t& field_offset) { + const auto position = point_index * point_step + field_offset; + if (data.size () >= (position + sizeof(T))) + return reinterpret_cast(data[position]); + else + throw std::out_of_range("PCLPointCloud2::at"); + } }; // struct PCLPointCloud2 using PCLPointCloud2Ptr = PCLPointCloud2::Ptr; diff --git a/common/include/pcl/PCLPointField.h b/common/include/pcl/PCLPointField.h index 73d82973..67acf07b 100644 --- a/common/include/pcl/PCLPointField.h +++ b/common/include/pcl/PCLPointField.h @@ -17,12 +17,15 @@ namespace pcl std::uint8_t datatype = 0; uindex_t count = 0; - enum PointFieldTypes { INT8 = traits::asEnum_v, + enum PointFieldTypes { BOOL = traits::asEnum_v, + INT8 = traits::asEnum_v, UINT8 = traits::asEnum_v, INT16 = traits::asEnum_v, UINT16 = traits::asEnum_v, INT32 = traits::asEnum_v, UINT32 = traits::asEnum_v, + INT64 = traits::asEnum_v, + UINT64 = traits::asEnum_v, FLOAT32 = traits::asEnum_v, FLOAT64 = traits::asEnum_v}; @@ -41,7 +44,17 @@ namespace pcl s << "offset: "; s << " " << v.offset << std::endl; s << "datatype: "; - s << " " << v.datatype << std::endl; + switch(v.datatype) { + case ::pcl::PCLPointField::PointFieldTypes::INT8: s << " INT8" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::UINT8: s << " UINT8" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::INT16: s << " INT16" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::UINT16: s << " UINT16" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::INT32: s << " INT32" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::UINT32: s << " UINT32" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::FLOAT32: s << " FLOAT32" << std::endl; break; + case ::pcl::PCLPointField::PointFieldTypes::FLOAT64: s << " FLOAT64" << std::endl; break; + default: s << " " << static_cast(v.datatype) << std::endl; + } s << "count: "; s << " " << v.count << std::endl; return (s); diff --git a/common/include/pcl/PointIndices.h b/common/include/pcl/PointIndices.h index 53e19dfb..77096c34 100644 --- a/common/include/pcl/PointIndices.h +++ b/common/include/pcl/PointIndices.h @@ -13,8 +13,7 @@ namespace pcl using Ptr = shared_ptr< ::pcl::PointIndices>; using ConstPtr = shared_ptr; - PointIndices () - {} + PointIndices () = default; ::pcl::PCLHeader header; diff --git a/common/include/pcl/PolygonMesh.h b/common/include/pcl/PolygonMesh.h index 55d69995..dd15962f 100644 --- a/common/include/pcl/PolygonMesh.h +++ b/common/include/pcl/PolygonMesh.h @@ -13,8 +13,7 @@ namespace pcl { struct PolygonMesh { - PolygonMesh () - {} + PolygonMesh () = default; ::pcl::PCLHeader header; @@ -57,7 +56,7 @@ namespace pcl return true; } - /** \brief Concatenate two pcl::PCLPointCloud2 + /** \brief Concatenate two pcl::PolygonMesh * \param[in] mesh1 the first input mesh * \param[in] mesh2 the second input mesh * \param[out] mesh_out the resultant output mesh diff --git a/common/include/pcl/TextureMesh.h b/common/include/pcl/TextureMesh.h index 88119906..3a819b8c 100644 --- a/common/include/pcl/TextureMesh.h +++ b/common/include/pcl/TextureMesh.h @@ -91,9 +91,17 @@ namespace pcl pcl::PCLHeader header; - std::vector > tex_polygons; // polygon which is mapped with specific texture defined in TexMaterial - std::vector > > tex_coordinates; // UV coordinates - std::vector tex_materials; // define texture material + /** \brief polygon which is mapped with specific texture defined in TexMaterial */ + std::vector > tex_polygons; + /** \brief UV coordinates */ + std::vector > > tex_coordinates; + /** \brief define texture material */ + std::vector tex_materials; + /** \brief Specifies which texture coordinates from tex_coordinates each polygon/face uses. + * The vectors must have the same sizes as in tex_polygons, but the pcl::Vertices + * may be empty for those polygons/faces that do not use coordinates. + */ + std::vector > tex_coord_indices; public: using Ptr = shared_ptr; diff --git a/common/include/pcl/Vertices.h b/common/include/pcl/Vertices.h index fe5b5d84..388b24e2 100644 --- a/common/include/pcl/Vertices.h +++ b/common/include/pcl/Vertices.h @@ -13,8 +13,7 @@ namespace pcl */ struct Vertices { - Vertices () - {} + Vertices () = default; Indices vertices; diff --git a/common/include/pcl/cloud_iterator.h b/common/include/pcl/cloud_iterator.h index 11908bb3..3300292c 100644 --- a/common/include/pcl/cloud_iterator.h +++ b/common/include/pcl/cloud_iterator.h @@ -89,7 +89,7 @@ namespace pcl class Iterator { public: - virtual ~Iterator () {} + virtual ~Iterator () = default; virtual void operator ++ () = 0; @@ -158,7 +158,7 @@ namespace pcl class Iterator { public: - virtual ~Iterator () {} + virtual ~Iterator () = default; virtual void operator ++ () = 0; diff --git a/common/include/pcl/common/common.h b/common/include/pcl/common/common.h index f21f0968..af32634c 100644 --- a/common/include/pcl/common/common.h +++ b/common/include/pcl/common/common.h @@ -282,7 +282,12 @@ namespace pcl * \ingroup common */ template inline auto - computeMedian (IteratorT begin, IteratorT end, Functor f) noexcept -> typename std::result_of::type + computeMedian (IteratorT begin, IteratorT end, Functor f) noexcept -> + #if __cpp_lib_is_invocable + std::invoke_result_t + #else + std::result_of_t + #endif { const std::size_t size = std::distance(begin, end); const std::size_t mid = size/2; diff --git a/common/include/pcl/common/fft/kiss_fft.h b/common/include/pcl/common/fft/kiss_fft.h index e7dc9e20..cff03e53 100644 --- a/common/include/pcl/common/fft/kiss_fft.h +++ b/common/include/pcl/common/fft/kiss_fft.h @@ -1,12 +1,12 @@ #pragma once -#include -#include -#include -#include - #include +#include // NOLINT +#include // NOLINT +#include // NOLINT +#include // NOLINT + #ifdef __cplusplus extern "C" { #endif @@ -49,12 +49,12 @@ extern "C" { # endif #endif -typedef struct { +typedef struct { // NOLINT kiss_fft_scalar r; kiss_fft_scalar i; }kiss_fft_cpx; -typedef struct kiss_fft_state* kiss_fft_cfg; +typedef struct kiss_fft_state* kiss_fft_cfg; // NOLINT /* * kiss_fft_alloc diff --git a/common/include/pcl/common/fft/kiss_fftr.h b/common/include/pcl/common/fft/kiss_fftr.h index e6d1e8d3..49c5ee01 100644 --- a/common/include/pcl/common/fft/kiss_fftr.h +++ b/common/include/pcl/common/fft/kiss_fftr.h @@ -14,7 +14,7 @@ extern "C" { */ -typedef struct kiss_fftr_state *kiss_fftr_cfg; +typedef struct kiss_fftr_state *kiss_fftr_cfg; // NOLINT kiss_fftr_cfg PCL_EXPORTS kiss_fftr_alloc(int nfft,int inverse_fft,void * mem, size_t * lenmem); diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp index 9f932b97..f83b12ca 100644 --- a/common/include/pcl/common/impl/centroid.hpp +++ b/common/include/pcl/common/impl/centroid.hpp @@ -56,10 +56,9 @@ template inline unsigned int compute3DCentroid (ConstCloudIterator &cloud_iterator, Eigen::Matrix ¢roid) { - // Initialize to 0 - centroid.setZero (); + Eigen::Matrix accumulator {0, 0, 0, 0}; - unsigned cp = 0; + unsigned int cp = 0; // For each point in the cloud // If the data is dense, we don't need to check for NaN @@ -68,15 +67,19 @@ compute3DCentroid (ConstCloudIterator &cloud_iterator, // Check if the point is invalid if (pcl::isFinite (*cloud_iterator)) { - centroid[0] += cloud_iterator->x; - centroid[1] += cloud_iterator->y; - centroid[2] += cloud_iterator->z; + accumulator[0] += cloud_iterator->x; + accumulator[1] += cloud_iterator->y; + accumulator[2] += cloud_iterator->z; ++cp; } ++cloud_iterator; } - centroid /= static_cast (cp); - centroid[3] = 1; + + if (cp > 0) { + centroid = accumulator; + centroid /= static_cast (cp); + centroid[3] = 1; + } return (cp); } @@ -88,12 +91,12 @@ compute3DCentroid (const pcl::PointCloud &cloud, if (cloud.empty ()) return (0); - // Initialize to 0 - centroid.setZero (); // For each point in the cloud // If the data is dense, we don't need to check for NaN if (cloud.is_dense) { + // Initialize to 0 + centroid.setZero (); for (const auto& point: cloud) { centroid[0] += point.x; @@ -106,20 +109,24 @@ compute3DCentroid (const pcl::PointCloud &cloud, return (static_cast (cloud.size ())); } // NaN or Inf values could exist => check for them - unsigned cp = 0; + unsigned int cp = 0; + Eigen::Matrix accumulator {0, 0, 0, 0}; for (const auto& point: cloud) { // Check if the point is invalid if (!isFinite (point)) continue; - centroid[0] += point.x; - centroid[1] += point.y; - centroid[2] += point.z; + accumulator[0] += point.x; + accumulator[1] += point.y; + accumulator[2] += point.z; ++cp; } - centroid /= static_cast (cp); - centroid[3] = 1; + if (cp > 0) { + centroid = accumulator; + centroid /= static_cast (cp); + centroid[3] = 1; + } return (cp); } @@ -133,11 +140,11 @@ compute3DCentroid (const pcl::PointCloud &cloud, if (indices.empty ()) return (0); - // Initialize to 0 - centroid.setZero (); // If the data is dense, we don't need to check for NaN if (cloud.is_dense) { + // Initialize to 0 + centroid.setZero (); for (const auto& index : indices) { centroid[0] += cloud[index].x; @@ -149,20 +156,24 @@ compute3DCentroid (const pcl::PointCloud &cloud, return (static_cast (indices.size ())); } // NaN or Inf values could exist => check for them - unsigned cp = 0; + Eigen::Matrix accumulator {0, 0, 0, 0}; + unsigned int cp = 0; for (const auto& index : indices) { // Check if the point is invalid if (!isFinite (cloud [index])) continue; - centroid[0] += cloud[index].x; - centroid[1] += cloud[index].y; - centroid[2] += cloud[index].z; + accumulator[0] += cloud[index].x; + accumulator[1] += cloud[index].y; + accumulator[2] += cloud[index].z; ++cp; } - centroid /= static_cast (cp); - centroid[3] = 1; + if (cp > 0) { + centroid = accumulator; + centroid /= static_cast (cp); + centroid[3] = 1; + } return (cp); } @@ -184,13 +195,11 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, if (cloud.empty ()) return (0); - // Initialize to 0 - covariance_matrix.setZero (); - unsigned point_count; // If the data is dense, we don't need to check for NaN if (cloud.is_dense) { + covariance_matrix.setZero (); point_count = static_cast (cloud.size ()); // For each point in the cloud for (const auto& point: cloud) @@ -214,6 +223,8 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, // NaN or Inf values could exist => check for them else { + Eigen::Matrix temp_covariance_matrix; + temp_covariance_matrix.setZero(); point_count = 0; // For each point in the cloud for (const auto& point: cloud) @@ -227,17 +238,23 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, pt[1] = point.y - centroid[1]; pt[2] = point.z - centroid[2]; - covariance_matrix (1, 1) += pt.y () * pt.y (); - covariance_matrix (1, 2) += pt.y () * pt.z (); + temp_covariance_matrix (1, 1) += pt.y () * pt.y (); + temp_covariance_matrix (1, 2) += pt.y () * pt.z (); - covariance_matrix (2, 2) += pt.z () * pt.z (); + temp_covariance_matrix (2, 2) += pt.z () * pt.z (); pt *= pt.x (); - covariance_matrix (0, 0) += pt.x (); - covariance_matrix (0, 1) += pt.y (); - covariance_matrix (0, 2) += pt.z (); + temp_covariance_matrix (0, 0) += pt.x (); + temp_covariance_matrix (0, 1) += pt.y (); + temp_covariance_matrix (0, 2) += pt.z (); ++point_count; } + if (point_count > 0) { + covariance_matrix = temp_covariance_matrix; + } + } + if (point_count == 0) { + return 0; } covariance_matrix (1, 0) = covariance_matrix (0, 1); covariance_matrix (2, 0) = covariance_matrix (0, 2); @@ -268,13 +285,11 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, if (indices.empty ()) return (0); - // Initialize to 0 - covariance_matrix.setZero (); - std::size_t point_count; // If the data is dense, we don't need to check for NaN if (cloud.is_dense) { + covariance_matrix.setZero (); point_count = indices.size (); // For each point in the cloud for (const auto& idx: indices) @@ -298,6 +313,8 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, // NaN or Inf values could exist => check for them else { + Eigen::Matrix temp_covariance_matrix; + temp_covariance_matrix.setZero (); point_count = 0; // For each point in the cloud for (const auto &index : indices) @@ -311,17 +328,23 @@ computeCovarianceMatrix (const pcl::PointCloud &cloud, pt[1] = cloud[index].y - centroid[1]; pt[2] = cloud[index].z - centroid[2]; - covariance_matrix (1, 1) += pt.y () * pt.y (); - covariance_matrix (1, 2) += pt.y () * pt.z (); + temp_covariance_matrix (1, 1) += pt.y () * pt.y (); + temp_covariance_matrix (1, 2) += pt.y () * pt.z (); - covariance_matrix (2, 2) += pt.z () * pt.z (); + temp_covariance_matrix (2, 2) += pt.z () * pt.z (); pt *= pt.x (); - covariance_matrix (0, 0) += pt.x (); - covariance_matrix (0, 1) += pt.y (); - covariance_matrix (0, 2) += pt.z (); + temp_covariance_matrix (0, 0) += pt.x (); + temp_covariance_matrix (0, 1) += pt.y (); + temp_covariance_matrix (0, 2) += pt.z (); ++point_count; } + if (point_count > 0) { + covariance_matrix = temp_covariance_matrix; + } + } + if (point_count == 0) { + return 0; } covariance_matrix (1, 0) = covariance_matrix (0, 1); covariance_matrix (2, 0) = covariance_matrix (0, 2); diff --git a/common/include/pcl/common/impl/common.hpp b/common/include/pcl/common/impl/common.hpp index 78df9b7a..b4e80224 100644 --- a/common/include/pcl/common/impl/common.hpp +++ b/common/include/pcl/common/impl/common.hpp @@ -40,7 +40,7 @@ #include #include -#include // for FLT_MAX +#include ////////////////////////////////////////////////////////////////////////////////////////////// inline double @@ -196,7 +196,7 @@ pcl::getPointsInBox (const pcl::PointCloud &cloud, template inline void pcl::getMaxDistance (const pcl::PointCloud &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt) { - float max_dist = -FLT_MAX; + float max_dist = std::numeric_limits::lowest(); int max_idx = -1; float dist; const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> (); @@ -244,7 +244,7 @@ template inline void pcl::getMaxDistance (const pcl::PointCloud &cloud, const Indices &indices, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt) { - float max_dist = -FLT_MAX; + float max_dist = std::numeric_limits::lowest(); int max_idx = -1; float dist; const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> (); @@ -304,8 +304,8 @@ pcl::getMinMax3D (const pcl::PointCloud &cloud, PointT &min_pt, PointT & template inline void pcl::getMinMax3D (const pcl::PointCloud &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) { - min_pt.setConstant (FLT_MAX); - max_pt.setConstant (-FLT_MAX); + min_pt.setConstant (std::numeric_limits::max()); + max_pt.setConstant (std::numeric_limits::lowest()); // If the data is dense, we don't need to check for NaN if (cloud.is_dense) @@ -348,8 +348,8 @@ template inline void pcl::getMinMax3D (const pcl::PointCloud &cloud, const Indices &indices, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt) { - min_pt.setConstant (FLT_MAX); - max_pt.setConstant (-FLT_MAX); + min_pt.setConstant (std::numeric_limits::max()); + max_pt.setConstant (std::numeric_limits::lowest()); // If the data is dense, we don't need to check for NaN if (cloud.is_dense) @@ -399,8 +399,8 @@ pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc template inline void pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p) { - min_p = FLT_MAX; - max_p = -FLT_MAX; + min_p = std::numeric_limits::max(); + max_p = std::numeric_limits::lowest(); for (int i = 0; i < len; ++i) { diff --git a/common/include/pcl/common/impl/io.hpp b/common/include/pcl/common/impl/io.hpp index d6d93aaf..be77397d 100644 --- a/common/include/pcl/common/impl/io.hpp +++ b/common/include/pcl/common/impl/io.hpp @@ -44,8 +44,6 @@ #include #include #include -#include - namespace pcl { @@ -367,8 +365,9 @@ copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud PointT* out_inner = out + cloud_out.width*top + left; for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) { - if (out_inner != in) - memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + if (out_inner != in) { + std::copy(in, in + cloud_in.width, out_inner); + } } } else @@ -394,8 +393,9 @@ copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) { - if (out_inner != in) - memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + if (out_inner != in) { + std::copy(in, in + cloud_in.width, out_inner); + } for (int j = 0; j < left; j++) out_inner[j - left] = in[padding[j]]; @@ -407,17 +407,15 @@ copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud for (int i = 0; i < top; i++) { int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type); - memcpy (out + i*cloud_out.width, - out + (j+top) * cloud_out.width, - sizeof (PointT) * cloud_out.width); + std::copy(out + (j+top) * cloud_out.width, out + (j+top) * cloud_out.width + cloud_out.width, + out + i*cloud_out.width); } for (int i = 0; i < bottom; i++) { int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type); - memcpy (out + (i + cloud_in.height + top)*cloud_out.width, - out + (j+top)*cloud_out.width, - cloud_out.width * sizeof (PointT)); + std::copy(out + (j+top)*cloud_out.width, out + (j+top)*cloud_out.width + cloud_out.width, + out + (i + cloud_in.height + top)*cloud_out.width); } } catch (pcl::BadArgumentException&) @@ -437,23 +435,23 @@ copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud for (std::uint32_t i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) { - if (out_inner != in) - memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + if (out_inner != in) { + std::copy(in, in + cloud_in.width, out_inner); + } - memcpy (out_inner - left, buff_ptr, left * sizeof (PointT)); - memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT)); + std::copy(buff_ptr, buff_ptr + left, out_inner - left); + std::copy(buff_ptr, buff_ptr + right, out_inner + cloud_in.width); } for (int i = 0; i < top; i++) { - memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT)); + std::copy(buff_ptr, buff_ptr + cloud_out.width, out + i*cloud_out.width); } for (int i = 0; i < bottom; i++) { - memcpy (out + (i + cloud_in.height + top)*cloud_out.width, - buff_ptr, - cloud_out.width * sizeof (PointT)); + std::copy(buff_ptr, buff_ptr + cloud_out.width, + out + (i + cloud_in.height + top)*cloud_out.width); } } } diff --git a/common/include/pcl/common/impl/spring.hpp b/common/include/pcl/common/impl/spring.hpp index b27f20c1..a6bf8bac 100644 --- a/common/include/pcl/common/impl/spring.hpp +++ b/common/include/pcl/common/impl/spring.hpp @@ -124,7 +124,7 @@ duplicateColumns (const PointCloud& input, PointCloud& output, for (std::size_t j = 0; j < old_height; ++j) for(std::size_t i = 0; i < amount; ++i) { - typename PointCloud::iterator start = output.begin () + (j * new_width); + auto start = output.begin () + (j * new_width); output.insert (start, *start); start = output.begin () + (j * new_width) + old_width + i; output.insert (start, *start); @@ -182,7 +182,7 @@ mirrorColumns (const PointCloud& input, PointCloud& output, for (std::size_t j = 0; j < old_height; ++j) for(std::size_t i = 0; i < amount; ++i) { - typename PointCloud::iterator start = output.begin () + (j * new_width); + auto start = output.begin () + (j * new_width); output.insert (start, *(start + 2*i)); start = output.begin () + (j * new_width) + old_width + 2*i; output.insert (start+1, *(start - 2*i)); @@ -254,7 +254,7 @@ deleteCols (const PointCloud& input, PointCloud& output, std::uint32_t new_width = old_width - 2 * amount; for(std::size_t j = 0; j < old_height; j++) { - typename PointCloud::iterator start = output.begin () + j * new_width; + auto start = output.begin () + j * new_width; output.erase (start, start + amount); start = output.begin () + (j+1) * new_width; output.erase (start, start + amount); diff --git a/common/include/pcl/common/impl/transforms.hpp b/common/include/pcl/common/impl/transforms.hpp index 7c51e8fb..6c884d16 100644 --- a/common/include/pcl/common/impl/transforms.hpp +++ b/common/include/pcl/common/impl/transforms.hpp @@ -306,6 +306,45 @@ transformPointCloud (const pcl::PointCloud &cloud_in, } +inline void +transformPointCloud(const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + const Eigen::Affine2f &transform, + bool copy_all_fields) + { + if (&cloud_in != &cloud_out) + { + cloud_out.header = cloud_in.header; + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.reserve (cloud_in.size ()); + if (copy_all_fields) + cloud_out.assign (cloud_in.begin (), cloud_in.end (), cloud_in.width); + else + cloud_out.resize (cloud_in.width, cloud_in.height); + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + } + if(cloud_in.is_dense) + { + for (std::size_t i = 0; i < cloud_out.size (); ++i) + { + cloud_out[i].getVector2fMap () = transform * cloud_in[i].getVector2fMap(); + } + } + else + { + for (std::size_t i = 0; i < cloud_out.size (); ++i) + { + if (!std::isfinite(cloud_in[i].x) || !std::isfinite(cloud_in[i].y)) + { + continue; + } + cloud_out[i].getVector2fMap () = transform * cloud_in[i].getVector2fMap(); + } + } + } + + template void transformPointCloudWithNormals (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, diff --git a/common/include/pcl/common/io.h b/common/include/pcl/common/io.h index 4e765e4e..5ff5370a 100644 --- a/common/include/pcl/common/io.h +++ b/common/include/pcl/common/io.h @@ -120,19 +120,24 @@ namespace pcl { switch (datatype) { - case pcl::PCLPointField::INT8: + case pcl::PCLPointField::BOOL: + return sizeof(bool); + + case pcl::PCLPointField::INT8: PCL_FALLTHROUGH case pcl::PCLPointField::UINT8: return (1); - case pcl::PCLPointField::INT16: + case pcl::PCLPointField::INT16: PCL_FALLTHROUGH case pcl::PCLPointField::UINT16: return (2); - case pcl::PCLPointField::INT32: - case pcl::PCLPointField::UINT32: + case pcl::PCLPointField::INT32: PCL_FALLTHROUGH + case pcl::PCLPointField::UINT32: PCL_FALLTHROUGH case pcl::PCLPointField::FLOAT32: return (4); + case pcl::PCLPointField::INT64: PCL_FALLTHROUGH + case pcl::PCLPointField::UINT64: PCL_FALLTHROUGH case pcl::PCLPointField::FLOAT64: return (8); @@ -151,13 +156,23 @@ namespace pcl /** \brief Obtains the type of the PCLPointField from a specific size and type * \param[in] size the size in bytes of the data field - * \param[in] type a char describing the type of the field ('F' = float, 'I' = signed, 'U' = unsigned) + * \param[in] type a char describing the type of the field ('B' = bool, 'F' = float, 'I' = signed, 'U' = unsigned) * \ingroup common */ inline int getFieldType (const int size, char type) { type = std::toupper (type, std::locale::classic ()); + + // extra logic for bool because its size is undefined + if (type == 'B') { + if (size == sizeof(bool)) { + return pcl::PCLPointField::BOOL; + } else { + return -1; + } + } + switch (size) { case 1: @@ -184,6 +199,10 @@ namespace pcl break; case 8: + if (type == 'I') + return (pcl::PCLPointField::INT64); + if (type == 'U') + return (pcl::PCLPointField::UINT64); if (type == 'F') return (pcl::PCLPointField::FLOAT64); break; @@ -200,19 +219,25 @@ namespace pcl { switch (type) { - case pcl::PCLPointField::INT8: - case pcl::PCLPointField::INT16: - case pcl::PCLPointField::INT32: + case pcl::PCLPointField::BOOL: + return ('B'); + + case pcl::PCLPointField::INT8: PCL_FALLTHROUGH + case pcl::PCLPointField::INT16: PCL_FALLTHROUGH + case pcl::PCLPointField::INT32: PCL_FALLTHROUGH + case pcl::PCLPointField::INT64: return ('I'); - case pcl::PCLPointField::UINT8: - case pcl::PCLPointField::UINT16: - case pcl::PCLPointField::UINT32: + case pcl::PCLPointField::UINT8: PCL_FALLTHROUGH + case pcl::PCLPointField::UINT16: PCL_FALLTHROUGH + case pcl::PCLPointField::UINT32: PCL_FALLTHROUGH + case pcl::PCLPointField::UINT64: return ('U'); - case pcl::PCLPointField::FLOAT32: + case pcl::PCLPointField::FLOAT32: PCL_FALLTHROUGH case pcl::PCLPointField::FLOAT64: return ('F'); + default: return ('?'); } @@ -319,10 +344,10 @@ namespace pcl pcl::PCLPointCloud2 &cloud_out); /** \brief Check if two given point types are the same or not. */ - template inline bool - isSamePointType () +template constexpr bool + isSamePointType() noexcept { - return (typeid (Point1T) == typeid (Point2T)); + return (std::is_same, remove_cvref_t>::value); } /** \brief Extract the indices of a given point cloud as a new point cloud diff --git a/common/include/pcl/common/time.h b/common/include/pcl/common/time.h index 7fbd4ea5..c5188af1 100644 --- a/common/include/pcl/common/time.h +++ b/common/include/pcl/common/time.h @@ -59,9 +59,7 @@ namespace pcl { public: /** \brief Constructor. */ - StopWatch () : start_time_ (std::chrono::steady_clock::now()) - { - } + StopWatch () = default; /** \brief Retrieve the time in milliseconds spent since the last call to \a reset(). */ inline double @@ -86,7 +84,7 @@ namespace pcl } protected: - std::chrono::time_point start_time_; + std::chrono::time_point start_time_ = std::chrono::steady_clock::now(); }; /** \brief Class to measure the time spent in a scope diff --git a/common/include/pcl/common/transforms.h b/common/include/pcl/common/transforms.h index 347643f2..5aa46b81 100644 --- a/common/include/pcl/common/transforms.h +++ b/common/include/pcl/common/transforms.h @@ -40,7 +40,6 @@ #pragma once #include -#include #include #include #include @@ -437,6 +436,20 @@ namespace pcl return (transformPointCloudWithNormals (cloud_in, cloud_out, offset, rotation, copy_all_fields)); } + /** \brief Apply an affine transform on a pointcloud having points of type PointXY + * \param[in] cloud_in the input point cloud + * \param[out] cloud_out the resultant output point cloud + * \param[in] transform an affine transformation + * \param[in] copy_all_fields flag that controls whether the contents of the fields + * (other than x, y, z) should be copied into the new transformed cloud + * \ingroup common + */ + void + transformPointCloud(const pcl::PointCloud& cloud_in, + pcl::PointCloud& cloud_out, + const Eigen::Affine2f& transform, + bool copy_all_fields = true); + /** \brief Transform a point with members x,y,z * \param[in] point the point to transform * \param[out] transform the transformation to apply diff --git a/common/include/pcl/console/time.h b/common/include/pcl/console/time.h index 07945323..38b028d8 100644 --- a/common/include/pcl/console/time.h +++ b/common/include/pcl/console/time.h @@ -50,7 +50,7 @@ namespace pcl { public: - TicToc () {} + TicToc () = default; void tic () diff --git a/common/include/pcl/conversions.h b/common/include/pcl/conversions.h index f60d78bb..7e9fdf40 100644 --- a/common/include/pcl/conversions.h +++ b/common/include/pcl/conversions.h @@ -51,7 +51,8 @@ #include #include -#include +#include +#include namespace pcl { @@ -190,7 +191,7 @@ namespace pcl // Should usually be able to copy all rows at once if (msg.row_step == cloud_row_step) { - memcpy (cloud_data, msg_data, msg.data.size ()); + std::copy(msg.data.cbegin(), msg.data.cend(), cloud_data); } else { @@ -210,7 +211,8 @@ namespace pcl const std::uint8_t* msg_data = row_data + col * msg.point_step; for (const detail::FieldMapping& mapping : field_map) { - memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); + std::copy(msg_data + mapping.serialized_offset, msg_data + mapping.serialized_offset + mapping.size, + cloud_data + mapping.struct_offset); } cloud_data += sizeof (PointT); } @@ -339,7 +341,7 @@ namespace pcl for (std::size_t x = 0; x < cloud.width; x++, rgb_offset += point_step) { std::uint8_t * pixel = &(msg.data[y * msg.step + x * 3]); - memcpy (pixel, &(cloud.data[rgb_offset]), 3 * sizeof (std::uint8_t)); + std::copy(&cloud.data[rgb_offset], &cloud.data[rgb_offset] + 3, pixel); } } } diff --git a/common/include/pcl/correspondence.h b/common/include/pcl/correspondence.h index 0b8d1f8b..825e2055 100644 --- a/common/include/pcl/correspondence.h +++ b/common/include/pcl/correspondence.h @@ -71,7 +71,7 @@ namespace pcl }; /** \brief Standard constructor. - * Sets \ref index_query to 0, \ref index_match to -1, and \ref distance to FLT_MAX. + * Sets \ref index_query to 0, \ref index_match to -1, and \ref distance to std::numeric_limits::max(). */ inline Correspondence () = default; diff --git a/common/include/pcl/for_each_type.h b/common/include/pcl/for_each_type.h index 9e0a6af0..fc5ca350 100644 --- a/common/include/pcl/for_each_type.h +++ b/common/include/pcl/for_each_type.h @@ -74,7 +74,7 @@ namespace pcl { using arg = typename boost::mpl::deref::type; -#if (defined _WIN32 && defined _MSC_VER) +#if (defined _WIN32 && defined _MSC_VER && !defined(__clang__)) boost::mpl::aux::unwrap (f, 0).operator() (); #else boost::mpl::aux::unwrap (f, 0).template operator() (); diff --git a/common/include/pcl/impl/cloud_iterator.hpp b/common/include/pcl/impl/cloud_iterator.hpp index 397419e7..c7671b82 100644 --- a/common/include/pcl/impl/cloud_iterator.hpp +++ b/common/include/pcl/impl/cloud_iterator.hpp @@ -56,9 +56,7 @@ namespace pcl { } - ~DefaultIterator () - { - } + ~DefaultIterator () = default; void operator ++ () { @@ -130,7 +128,7 @@ namespace pcl { } - virtual ~IteratorIdx () {} + virtual ~IteratorIdx () = default; void operator ++ () { @@ -196,9 +194,7 @@ namespace pcl { } - ~DefaultConstIterator () - { - } + ~DefaultConstIterator () override = default; void operator ++ () override { @@ -272,7 +268,7 @@ namespace pcl { } - ~ConstIteratorIdx () {} + ~ConstIteratorIdx () override = default; void operator ++ () override { diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp index f17b1bc9..a3aee983 100644 --- a/common/include/pcl/impl/point_types.hpp +++ b/common/include/pcl/impl/point_types.hpp @@ -218,7 +218,9 @@ namespace pcl static constexpr int descriptorSize_v = descriptorSize::value; } } - + + using Vector2fMap = Eigen::Map; + using Vector2fMapConst = const Eigen::Map; using Array3fMap = Eigen::Map; using Array3fMapConst = const Eigen::Map; using Array4fMap = Eigen::Map; @@ -246,6 +248,8 @@ namespace pcl }; #define PCL_ADD_EIGEN_MAPS_POINT4D \ + inline pcl::Vector2fMap getVector2fMap () { return (pcl::Vector2fMap (data)); } \ + inline pcl::Vector2fMapConst getVector2fMap () const { return (pcl::Vector2fMapConst (data)); } \ inline pcl::Vector3fMap getVector3fMap () { return (pcl::Vector3fMap (data)); } \ inline pcl::Vector3fMapConst getVector3fMap () const { return (pcl::Vector3fMapConst (data)); } \ inline pcl::Vector4fMap getVector4fMap () { return (pcl::Vector4fMap (data)); } \ @@ -345,15 +349,11 @@ namespace pcl */ struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ { - inline PointXYZ (const _PointXYZ &p): PointXYZ(p.x, p.y, p.z) {} + inline constexpr PointXYZ (const _PointXYZ &p): PointXYZ(p.x, p.y, p.z) {} - inline PointXYZ (): PointXYZ(0.f, 0.f, 0.f) {} + inline constexpr PointXYZ (): PointXYZ(0.f, 0.f, 0.f) {} - inline PointXYZ (float _x, float _y, float _z) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - } + inline constexpr PointXYZ (float _x, float _y, float _z) : _PointXYZ{{{_x, _y, _z, 1.f}}} {} friend std::ostream& operator << (std::ostream& os, const PointXYZ& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -390,18 +390,11 @@ namespace pcl */ struct RGB: public _RGB { - inline RGB (const _RGB &p) - { - rgba = p.rgba; - } + inline constexpr RGB (const _RGB &p) : RGB{p.r, p.g, p.b, p.a} {} - inline RGB (): RGB(0, 0, 0) {} + inline constexpr RGB (): RGB(0, 0, 0) {} - inline RGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) - { - r = _r; g = _g; b = _b; - a = 255; - } + inline constexpr RGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a = 255) : _RGB{{{{_b, _g, _r, _a}}}} {} friend std::ostream& operator << (std::ostream& os, const RGB& p); }; @@ -418,15 +411,9 @@ namespace pcl */ struct Intensity: public _Intensity { - inline Intensity (const _Intensity &p) - { - intensity = p.intensity; - } + inline constexpr Intensity (const _Intensity &p) : Intensity{p.intensity} {} - inline Intensity (float _intensity = 0.f) - { - intensity = _intensity; - } + inline constexpr Intensity (float _intensity = 0.f) : _Intensity{_intensity} {} friend std::ostream& operator << (std::ostream& os, const Intensity& p); }; @@ -444,18 +431,12 @@ namespace pcl */ struct Intensity8u: public _Intensity8u { - inline Intensity8u (const _Intensity8u &p) - { - intensity = p.intensity; - } + inline constexpr Intensity8u (const _Intensity8u &p) : Intensity8u{p.intensity} {} - inline Intensity8u (std::uint8_t _intensity = 0) - { - intensity = _intensity; - } + inline constexpr Intensity8u (std::uint8_t _intensity = 0) : _Intensity8u{_intensity} {} #if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 - operator unsigned char() const + inline constexpr operator unsigned char() const { return intensity; } @@ -476,15 +457,9 @@ namespace pcl */ struct Intensity32u: public _Intensity32u { - inline Intensity32u (const _Intensity32u &p) - { - intensity = p.intensity; - } + inline constexpr Intensity32u (const _Intensity32u &p) : Intensity32u{p.intensity} {} - inline Intensity32u (std::uint32_t _intensity = 0) - { - intensity = _intensity; - } + inline constexpr Intensity32u (std::uint32_t _intensity = 0) : _Intensity32u{_intensity} {} friend std::ostream& operator << (std::ostream& os, const Intensity32u& p); }; @@ -509,21 +484,12 @@ namespace pcl PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZI& p); struct PointXYZI : public _PointXYZI { - inline PointXYZI (const _PointXYZI &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - intensity = p.intensity; - } + inline constexpr PointXYZI (const _PointXYZI &p) : PointXYZI{p.x, p.y, p.z, p.intensity} {} - inline PointXYZI (float _intensity = 0.f): PointXYZI(0.f, 0.f, 0.f, _intensity) {} - - inline PointXYZI (float _x, float _y, float _z, float _intensity = 0.f) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - intensity = _intensity; - } + inline constexpr PointXYZI (float _intensity = 0.f) : PointXYZI(0.f, 0.f, 0.f, _intensity) {} + inline constexpr PointXYZI (float _x, float _y, float _z, float _intensity = 0.f) : _PointXYZI{{{_x, _y, _z, 1.0f}}, {{_intensity}}} {} + friend std::ostream& operator << (std::ostream& os, const PointXYZI& p); }; @@ -538,20 +504,11 @@ namespace pcl PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZL& p); struct PointXYZL : public _PointXYZL { - inline PointXYZL (const _PointXYZL &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - label = p.label; - } + inline constexpr PointXYZL (const _PointXYZL &p) : PointXYZL{p.x, p.y, p.z, p.label} {} - inline PointXYZL (std::uint32_t _label = 0): PointXYZL(0.f, 0.f, 0.f, _label) {} + inline constexpr PointXYZL (std::uint32_t _label = 0) : PointXYZL(0.f, 0.f, 0.f, _label) {} - inline PointXYZL (float _x, float _y, float _z, std::uint32_t _label = 0) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - label = _label; - } + inline constexpr PointXYZL (float _x, float _y, float _z, std::uint32_t _label = 0) : _PointXYZL{{{_x, _y, _z, 1.0f}}, _label} {} friend std::ostream& operator << (std::ostream& os, const PointXYZL& p); }; @@ -562,7 +519,7 @@ namespace pcl { std::uint32_t label = 0; - Label (std::uint32_t _label = 0): label(_label) {} + inline constexpr Label (std::uint32_t _label = 0): label(_label) {} friend std::ostream& operator << (std::ostream& os, const Label& p); }; @@ -598,27 +555,18 @@ namespace pcl */ struct EIGEN_ALIGN16 PointXYZRGBA : public _PointXYZRGBA { - inline PointXYZRGBA (const _PointXYZRGBA &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - rgba = p.rgba; - } + inline constexpr PointXYZRGBA (const _PointXYZRGBA &p) : PointXYZRGBA{p.x, p.y, p.z, p.r, p.g, p.b, p.a} {} - inline PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 255) {} + inline constexpr PointXYZRGBA (): PointXYZRGBA (0, 0, 0, 255) {} - inline PointXYZRGBA (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a): + inline constexpr PointXYZRGBA (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a): PointXYZRGBA (0.f, 0.f, 0.f, _r, _g, _b, _a) {} - inline PointXYZRGBA (float _x, float _y, float _z): + inline constexpr PointXYZRGBA (float _x, float _y, float _z): PointXYZRGBA (_x, _y, _z, 0, 0, 0, 255) {} - inline PointXYZRGBA (float _x, float _y, float _z, std::uint8_t _r, - std::uint8_t _g, std::uint8_t _b, std::uint8_t _a) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - r = _r; g = _g; b = _b; a = _a; - } + inline constexpr PointXYZRGBA (float _x, float _y, float _z, std::uint8_t _r, + std::uint8_t _g, std::uint8_t _b, std::uint8_t _a) : _PointXYZRGBA{{{_x, _y, _z, 1.0f}}, {{{_b, _g, _r, _a}}}} {} friend std::ostream& operator << (std::ostream& os, const PointXYZRGBA& p); }; @@ -673,28 +621,19 @@ namespace pcl */ struct EIGEN_ALIGN16 PointXYZRGB : public _PointXYZRGB { - inline PointXYZRGB (const _PointXYZRGB &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - rgb = p.rgb; - } + inline constexpr PointXYZRGB (const _PointXYZRGB &p) : PointXYZRGB{p.x, p.y, p.z, p.r, p.g, p.b} {} - inline PointXYZRGB (): PointXYZRGB (0.f, 0.f, 0.f) {} + inline constexpr PointXYZRGB (): PointXYZRGB (0.f, 0.f, 0.f) {} - inline PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + inline constexpr PointXYZRGB (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): PointXYZRGB (0.f, 0.f, 0.f, _r, _g, _b) {} - inline PointXYZRGB (float _x, float _y, float _z): + inline constexpr PointXYZRGB (float _x, float _y, float _z): PointXYZRGB (_x, _y, _z, 0, 0, 0) {} - inline PointXYZRGB (float _x, float _y, float _z, - std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - r = _r; g = _g; b = _b; - a = 255; - } + inline constexpr PointXYZRGB (float _x, float _y, float _z, + std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + _PointXYZRGB{{{_x, _y, _z, 1.0f}}, {{{_b, _g, _r, 255}}}} {} friend std::ostream& operator << (std::ostream& os, const PointXYZRGB& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -704,32 +643,21 @@ namespace pcl PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p); struct EIGEN_ALIGN16 PointXYZRGBL : public _PointXYZRGBL { - inline PointXYZRGBL (const _PointXYZRGBL &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - rgba = p.rgba; - label = p.label; - } + inline constexpr PointXYZRGBL (const _PointXYZRGBL &p) : PointXYZRGBL{p.x, p.y, p.z, p.r, p.g, p.b, p.label, p.a} {} - inline PointXYZRGBL (std::uint32_t _label = 0): + inline constexpr PointXYZRGBL (std::uint32_t _label = 0): PointXYZRGBL (0.f, 0.f, 0.f, 0, 0, 0, _label) {} - inline PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + inline constexpr PointXYZRGBL (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): PointXYZRGBL (0.f, 0.f, 0.f, _r, _g, _b) {} - inline PointXYZRGBL (float _x, float _y, float _z): + inline constexpr PointXYZRGBL (float _x, float _y, float _z): PointXYZRGBL (_x, _y, _z, 0, 0, 0) {} - inline PointXYZRGBL (float _x, float _y, float _z, + inline constexpr PointXYZRGBL (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, - std::uint32_t _label = 0) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - r = _r; g = _g; b = _b; - a = 255; - label = _label; - } + std::uint32_t _label = 0, std::uint8_t _a = 255) : + _PointXYZRGBL{{{_x, _y, _z, 1.0f}}, {{{_b, _g, _r, _a}}}, _label} {} friend std::ostream& operator << (std::ostream& os, const PointXYZRGBL& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -758,19 +686,13 @@ namespace pcl */ struct PointXYZLAB : public _PointXYZLAB { - inline PointXYZLAB (const _PointXYZLAB &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - L = p.L; a = p.a; b = p.b; - } + inline constexpr PointXYZLAB (const _PointXYZLAB &p) : PointXYZLAB{p.x, p.y, p.z, p.L, p.a, p.b} {} - inline PointXYZLAB() - { - x = y = z = 0.0f; - data[3] = 1.0f; // important for homogeneous coordinates - L = a = b = 0.0f; - data_lab[3] = 0.0f; - } + inline constexpr PointXYZLAB() : PointXYZLAB{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} {} + + inline constexpr PointXYZLAB (float _x, float _y, float _z, + float _L, float _a, float _b) : + _PointXYZLAB{ {{_x, _y, _z, 1.0f}}, {{_L, _a, _b}} } {} friend std::ostream& operator << (std::ostream& os, const PointXYZLAB& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -796,47 +718,46 @@ namespace pcl PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZHSV& p); struct EIGEN_ALIGN16 PointXYZHSV : public _PointXYZHSV { - inline PointXYZHSV (const _PointXYZHSV &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - h = p.h; s = p.s; v = p.v; - } + inline constexpr PointXYZHSV (const _PointXYZHSV &p) : + PointXYZHSV{p.x, p.y, p.z, p.h, p.s, p.v} {} - inline PointXYZHSV (): PointXYZHSV (0.f, 0.f, 0.f) {} + inline constexpr PointXYZHSV (): PointXYZHSV (0.f, 0.f, 0.f) {} // @TODO: Use strong types?? // This is a dangerous type, doesn't behave like others - inline PointXYZHSV (float _h, float _s, float _v): + inline constexpr PointXYZHSV (float _h, float _s, float _v): PointXYZHSV (0.f, 0.f, 0.f, _h, _s, _v) {} - inline PointXYZHSV (float _x, float _y, float _z, - float _h, float _s, float _v) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - h = _h; s = _s; v = _v; - data_c[3] = 0; - } + inline constexpr PointXYZHSV (float _x, float _y, float _z, + float _h, float _s, float _v) : + _PointXYZHSV{{{_x, _y, _z, 1.0f}}, {{_h, _s, _v}}} {} friend std::ostream& operator << (std::ostream& os, const PointXYZHSV& p); PCL_MAKE_ALIGNED_OPERATOR_NEW }; - - PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXY& p); /** \brief A 2D point structure representing Euclidean xy coordinates. * \ingroup common */ struct PointXY { - float x = 0.f; - float y = 0.f; - - inline PointXY() = default; - - inline PointXY(float _x, float _y): x(_x), y(_y) {} + union + { + float data[2]; + struct + { + float x; + float y; + }; + }; + inline constexpr PointXY(float _x, float _y): x(_x), y(_y) {} + inline constexpr PointXY(): x(0.0f), y(0.0f) {} + + inline pcl::Vector2fMap getVector2fMap () { return (pcl::Vector2fMap (data)); } + inline pcl::Vector2fMapConst getVector2fMap () const { return (pcl::Vector2fMapConst (data)); } + friend std::ostream& operator << (std::ostream& os, const PointXY& p); }; @@ -850,9 +771,9 @@ namespace pcl float u = 0.f; float v = 0.f; - inline PointUV() = default; + inline constexpr PointUV() = default; - inline PointUV(float _u, float _v): u(_u), v(_v) {} + inline constexpr PointUV(float _u, float _v): u(_u), v(_v) {} friend std::ostream& operator << (std::ostream& os, const PointUV& p); }; @@ -898,21 +819,12 @@ namespace pcl */ struct Normal : public _Normal { - inline Normal (const _Normal &p) - { - normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; - data_n[3] = 0.0f; - curvature = p.curvature; - } + inline constexpr Normal (const _Normal &p) : Normal {p.normal_x, p.normal_y, p.normal_z, p.curvature} {} - inline Normal (float _curvature = 0.f): Normal (0.f, 0.f, 0.f, _curvature) {} + inline constexpr Normal (float _curvature = 0.f): Normal (0.f, 0.f, 0.f, _curvature) {} - inline Normal (float n_x, float n_y, float n_z, float _curvature = 0.f) - { - normal_x = n_x; normal_y = n_y; normal_z = n_z; - data_n[3] = 0.0f; - curvature = _curvature; - } + inline constexpr Normal (float n_x, float n_y, float n_z, float _curvature = 0.f) : + _Normal{{{n_x, n_y, n_z, 0.0f}}, {{_curvature}}} {} friend std::ostream& operator << (std::ostream& os, const Normal& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -931,19 +843,11 @@ namespace pcl */ struct EIGEN_ALIGN16 Axis : public _Axis { - inline Axis (const _Axis &p) - { - normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; - data_n[3] = 0.0f; - } + inline constexpr Axis (const _Axis &p) : Axis{p.normal_x, p.normal_y, p.normal_z} {} - inline Axis (): Axis (0.f, 0.f, 0.f) {} + inline constexpr Axis (): Axis (0.f, 0.f, 0.f) {} - inline Axis (float n_x, float n_y, float n_z) - { - normal_x = n_x; normal_y = n_y; normal_z = n_z; - data_n[3] = 0.0f; - } + inline constexpr Axis (float n_x, float n_y, float n_z) : _Axis{{{n_x, n_y, n_z, 0.0f}}} {} friend std::ostream& operator << (std::ostream& os, const Axis& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -971,26 +875,15 @@ namespace pcl */ struct PointNormal : public _PointNormal { - inline PointNormal (const _PointNormal &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; - curvature = p.curvature; - } + inline constexpr PointNormal (const _PointNormal &p) : PointNormal{p.x, p.y, p.z, p.normal_x, p.normal_y, p.normal_z, p.curvature} {} - inline PointNormal (float _curvature = 0.f): PointNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {} + inline constexpr PointNormal (float _curvature = 0.f): PointNormal (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _curvature) {} - inline PointNormal (float _x, float _y, float _z): + inline constexpr PointNormal (float _x, float _y, float _z): PointNormal (_x, _y, _z, 0.f, 0.f, 0.f, 0.f) {} - inline PointNormal (float _x, float _y, float _z, float n_x, float n_y, float n_z, float _curvature = 0.f) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - normal_x = n_x; normal_y = n_y; normal_z = n_z; - data_n[3] = 0.0f; - curvature = _curvature; - } + inline constexpr PointNormal (float _x, float _y, float _z, float n_x, float n_y, float n_z, float _curvature = 0.f) : + _PointNormal{{{_x, _y, _z, 1.0f}}, {{n_x, n_y, n_z, 0.0f}}, {{_curvature}}} {} friend std::ostream& operator << (std::ostream& os, const PointNormal& p); }; @@ -1045,37 +938,39 @@ namespace pcl */ struct PointXYZRGBNormal : public _PointXYZRGBNormal { - inline PointXYZRGBNormal (const _PointXYZRGBNormal &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; - curvature = p.curvature; - rgba = p.rgba; - } + inline constexpr PointXYZRGBNormal (const _PointXYZRGBNormal &p) : + PointXYZRGBNormal {p.x, p.y, p.z, p.r, p.g, p.b, p.a, p.normal_x, p.normal_y, p.normal_z, p.curvature} {} - inline PointXYZRGBNormal (float _curvature = 0.f): + inline constexpr PointXYZRGBNormal (float _curvature = 0.f): PointXYZRGBNormal (0.f, 0.f, 0.f, 0, 0, 0, 0.f, 0.f, 0.f, _curvature) {} - inline PointXYZRGBNormal (float _x, float _y, float _z): + inline constexpr PointXYZRGBNormal (float _x, float _y, float _z): PointXYZRGBNormal (_x, _y, _z, 0, 0, 0) {} - inline PointXYZRGBNormal (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + inline constexpr PointXYZRGBNormal (std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): PointXYZRGBNormal (0.f, 0.f, 0.f, _r, _g, _b) {} - inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): + inline constexpr PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b): PointXYZRGBNormal (_x, _y, _z, _r, _g, _b, 0.f, 0.f, 0.f) {} - inline PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, - float n_x, float n_y, float n_z, float _curvature = 0.f) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - r = _r; g = _g; b = _b; - a = 255; - normal_x = n_x; normal_y = n_y; normal_z = n_z; - data_n[3] = 0.f; - curvature = _curvature; - } + inline constexpr PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, + float n_x, float n_y, float n_z, float _curvature = 0.f) : + _PointXYZRGBNormal{ + {{_x, _y, _z, 1.0f}}, + {{n_x, n_y, n_z, 0.0f}}, + {{ {{{_b, _g, _r, 255u}}}, _curvature }} + } + {} + + inline constexpr PointXYZRGBNormal (float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, + std::uint8_t _a, float n_x, float n_y, float n_z, float _curvature = 0.f) : + _PointXYZRGBNormal{ + {{_x, _y, _z, 1.0f}}, + {{n_x, n_y, n_z, 0.0f}}, + {{ {{{_b, _g, _r, _a}}}, _curvature }} + } + {} + friend std::ostream& operator << (std::ostream& os, const PointXYZRGBNormal& p); }; @@ -1102,29 +997,22 @@ namespace pcl */ struct PointXYZINormal : public _PointXYZINormal { - inline PointXYZINormal (const _PointXYZINormal &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; - curvature = p.curvature; - intensity = p.intensity; - } + inline constexpr PointXYZINormal (const _PointXYZINormal &p) : + PointXYZINormal {p.x, p.y, p.z, p.intensity, p.normal_x, p.normal_y, p.normal_z, p.curvature} {} - inline PointXYZINormal (float _intensity = 0.f): PointXYZINormal (0.f, 0.f, 0.f, _intensity) {} + inline constexpr PointXYZINormal (float _intensity = 0.f): PointXYZINormal (0.f, 0.f, 0.f, _intensity) {} - inline PointXYZINormal (float _x, float _y, float _z, float _intensity = 0.f): + inline constexpr PointXYZINormal (float _x, float _y, float _z, float _intensity = 0.f): PointXYZINormal (_x, _y, _z, _intensity, 0.f, 0.f, 0.f) {} - inline PointXYZINormal (float _x, float _y, float _z, float _intensity, - float n_x, float n_y, float n_z, float _curvature = 0.f) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - intensity = _intensity; - normal_x = n_x; normal_y = n_y; normal_z = n_z; - data_n[3] = 0.f; - curvature = _curvature; - } + inline constexpr PointXYZINormal (float _x, float _y, float _z, float _intensity, + float n_x, float n_y, float n_z, float _curvature = 0.f) : + _PointXYZINormal{ + {{_x, _y, _z, 1.0f}}, + {{n_x, n_y, n_z, 0.0f}}, + {{_intensity, _curvature}} + } + {} friend std::ostream& operator << (std::ostream& os, const PointXYZINormal& p); }; @@ -1152,29 +1040,22 @@ namespace pcl */ struct PointXYZLNormal : public _PointXYZLNormal { - inline PointXYZLNormal (const _PointXYZLNormal &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - normal_x = p.normal_x; normal_y = p.normal_y; normal_z = p.normal_z; data_n[3] = 0.0f; - curvature = p.curvature; - label = p.label; - } + inline constexpr PointXYZLNormal (const _PointXYZLNormal &p) : + PointXYZLNormal {p.x, p.y, p.z, p.label, p.normal_x, p.normal_y, p.normal_z, p.curvature} {} - inline PointXYZLNormal (std::uint32_t _label = 0): PointXYZLNormal (0.f, 0.f, 0.f, _label) {} + inline constexpr PointXYZLNormal (std::uint32_t _label = 0u): PointXYZLNormal (0.f, 0.f, 0.f, _label) {} - inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label = 0.f): + inline constexpr PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label = 0u) : PointXYZLNormal (_x, _y, _z, _label, 0.f, 0.f, 0.f) {} - inline PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label, - float n_x, float n_y, float n_z, float _curvature = 0.f) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - label = _label; - normal_x = n_x; normal_y = n_y; normal_z = n_z; - data_n[3] = 0.f; - curvature = _curvature; - } + inline constexpr PointXYZLNormal (float _x, float _y, float _z, std::uint32_t _label, + float n_x, float n_y, float n_z, float _curvature = 0.f) : + _PointXYZLNormal{ + {{_x, _y, _z, 1.0f}}, + {{n_x, n_y, n_z, 0.0f}}, + {{_label, _curvature}} + } + {} friend std::ostream& operator << (std::ostream& os, const PointXYZLNormal& p); }; @@ -1202,20 +1083,12 @@ namespace pcl */ struct PointWithRange : public _PointWithRange { - inline PointWithRange (const _PointWithRange &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - range = p.range; - } + inline constexpr PointWithRange (const _PointWithRange &p) : PointWithRange{p.x, p.y, p.z, p.range} {} - inline PointWithRange (float _range = 0.f): PointWithRange (0.f, 0.f, 0.f, _range) {} + inline constexpr PointWithRange (float _range = 0.f): PointWithRange (0.f, 0.f, 0.f, _range) {} - inline PointWithRange (float _x, float _y, float _z, float _range = 0.f) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - range = _range; - } + inline constexpr PointWithRange (float _x, float _y, float _z, float _range = 0.f) : + _PointWithRange{{{_x, _y, _z, 1.0f}}, {{_range}}} {} friend std::ostream& operator << (std::ostream& os, const PointWithRange& p); }; @@ -1243,22 +1116,14 @@ namespace pcl */ struct EIGEN_ALIGN16 PointWithViewpoint : public _PointWithViewpoint { - inline PointWithViewpoint (const _PointWithViewpoint &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - vp_x = p.vp_x; vp_y = p.vp_y; vp_z = p.vp_z; - } + inline constexpr PointWithViewpoint (const _PointWithViewpoint &p) : PointWithViewpoint{p.x, p.y, p.z, p.vp_x, p.vp_y, p.vp_z} {} - inline PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {} + inline constexpr PointWithViewpoint (): PointWithViewpoint (0.f, 0.f, 0.f) {} - inline PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {} + inline constexpr PointWithViewpoint (float _x, float _y, float _z): PointWithViewpoint (_x, _y, _z, 0.f, 0.f, 0.f) {} - inline PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - vp_x = _vp_x; vp_y = _vp_y; vp_z = _vp_z; - } + inline constexpr PointWithViewpoint (float _x, float _y, float _z, float _vp_x, float _vp_y, float _vp_z) : + _PointWithViewpoint{{{_x, _y, _z, 1.0f}}, {{_vp_x, _vp_y, _vp_z}}} {} friend std::ostream& operator << (std::ostream& os, const PointWithViewpoint& p); }; @@ -1271,9 +1136,9 @@ namespace pcl { float j1 = 0.f, j2 = 0.f, j3 = 0.f; - inline MomentInvariants () = default; + inline constexpr MomentInvariants () = default; - inline MomentInvariants (float _j1, float _j2, float _j3): j1 (_j1), j2 (_j2), j3 (_j3) {} + inline constexpr MomentInvariants (float _j1, float _j2, float _j3): j1 (_j1), j2 (_j2), j3 (_j3) {} friend std::ostream& operator << (std::ostream& os, const MomentInvariants& p); }; @@ -1286,9 +1151,9 @@ namespace pcl { float r_min = 0.f, r_max = 0.f; - inline PrincipalRadiiRSD () = default; + inline constexpr PrincipalRadiiRSD () = default; - inline PrincipalRadiiRSD (float _r_min, float _r_max): r_min (_r_min), r_max (_r_max) {} + inline constexpr PrincipalRadiiRSD (float _r_min, float _r_max): r_min (_r_min), r_max (_r_max) {} friend std::ostream& operator << (std::ostream& os, const PrincipalRadiiRSD& p); }; @@ -1302,13 +1167,13 @@ namespace pcl std::uint8_t boundary_point = 0; #if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION <= 1101 - operator unsigned char() const + constexpr operator unsigned char() const { return boundary_point; } #endif - inline Boundary (std::uint8_t _boundary = 0): boundary_point (_boundary) {} + inline constexpr Boundary (std::uint8_t _boundary = 0): boundary_point (_boundary) {} friend std::ostream& operator << (std::ostream& os, const Boundary& p); }; @@ -1332,13 +1197,13 @@ namespace pcl float pc1 = 0.f; float pc2 = 0.f; - inline PrincipalCurvatures (): PrincipalCurvatures (0.f, 0.f) {} + inline constexpr PrincipalCurvatures (): PrincipalCurvatures (0.f, 0.f) {} - inline PrincipalCurvatures (float _pc1, float _pc2): PrincipalCurvatures (0.f, 0.f, 0.f, _pc1, _pc2) {} + inline constexpr PrincipalCurvatures (float _pc1, float _pc2): PrincipalCurvatures (0.f, 0.f, 0.f, _pc1, _pc2) {} - inline PrincipalCurvatures (float _x, float _y, float _z): PrincipalCurvatures (_x, _y, _z, 0.f, 0.f) {} + inline constexpr PrincipalCurvatures (float _x, float _y, float _z): PrincipalCurvatures (_x, _y, _z, 0.f, 0.f) {} - inline PrincipalCurvatures (float _x, float _y, float _z, float _pc1, float _pc2): + inline constexpr PrincipalCurvatures (float _x, float _y, float _z, float _pc1, float _pc2): principal_curvature_x (_x), principal_curvature_y (_y), principal_curvature_z (_z), pc1 (_pc1), pc2 (_pc2) {} friend std::ostream& operator << (std::ostream& os, const PrincipalCurvatures& p); @@ -1353,7 +1218,7 @@ namespace pcl float histogram[125] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline PFHSignature125 () = default; + inline constexpr PFHSignature125 () = default; friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p); }; @@ -1368,7 +1233,7 @@ namespace pcl float histogram[250] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline PFHRGBSignature250 () = default; + inline constexpr PFHRGBSignature250 () = default; friend std::ostream& operator << (std::ostream& os, const PFHRGBSignature250& p); }; @@ -1382,9 +1247,9 @@ namespace pcl float f1 = 0.f, f2 = 0.f, f3 = 0.f, f4 = 0.f; float alpha_m = 0.f; - inline PPFSignature (float _alpha = 0.f): PPFSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} + inline constexpr PPFSignature (float _alpha = 0.f): PPFSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} - inline PPFSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): + inline constexpr PPFSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), alpha_m (_alpha) {} friend std::ostream& operator << (std::ostream& os, const PPFSignature& p); @@ -1399,10 +1264,10 @@ namespace pcl float f1, f2, f3, f4, f5, f6, f7, f8, f9, f10; float alpha_m; - inline CPPFSignature (float _alpha = 0.f): + inline constexpr CPPFSignature (float _alpha = 0.f): CPPFSignature (0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, _alpha) {} - inline CPPFSignature (float _f1, float _f2, float _f3, float _f4, float _f5, float _f6, + inline constexpr CPPFSignature (float _f1, float _f2, float _f3, float _f4, float _f5, float _f6, float _f7, float _f8, float _f9, float _f10, float _alpha = 0.f): f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), f5 (_f5), f6 (_f6), f7 (_f7), f8 (_f8), f9 (_f9), f10 (_f10), alpha_m (_alpha) {} @@ -1420,12 +1285,12 @@ namespace pcl float r_ratio = 0.f, g_ratio = 0.f, b_ratio = 0.f; float alpha_m = 0.f; - inline PPFRGBSignature (float _alpha = 0.f): PPFRGBSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} + inline constexpr PPFRGBSignature (float _alpha = 0.f): PPFRGBSignature (0.f, 0.f, 0.f, 0.f, _alpha) {} - inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): + inline constexpr PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha = 0.f): PPFRGBSignature (_f1, _f2, _f3, _f4, _alpha, 0.f, 0.f, 0.f) {} - inline PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha, float _r, float _g, float _b): + inline constexpr PPFRGBSignature (float _f1, float _f2, float _f3, float _f4, float _alpha, float _r, float _g, float _b): f1 (_f1), f2 (_f2), f3 (_f3), f4 (_f4), r_ratio (_r), g_ratio (_g), b_ratio (_b), alpha_m (_alpha) {} friend std::ostream& operator << (std::ostream& os, const PPFRGBSignature& p); @@ -1440,7 +1305,7 @@ namespace pcl { float values[12] = {0.f}; - inline NormalBasedSignature12 () = default; + inline constexpr NormalBasedSignature12 () = default; friend std::ostream& operator << (std::ostream& os, const NormalBasedSignature12& p); }; @@ -1455,7 +1320,7 @@ namespace pcl float rf[9] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline ShapeContext1980 () = default; + inline constexpr ShapeContext1980 () = default; friend std::ostream& operator << (std::ostream& os, const ShapeContext1980& p); }; @@ -1470,7 +1335,7 @@ namespace pcl float rf[9] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline UniqueShapeContext1960 () = default; + inline constexpr UniqueShapeContext1960 () = default; friend std::ostream& operator << (std::ostream& os, const UniqueShapeContext1960& p); }; @@ -1485,7 +1350,7 @@ namespace pcl float rf[9] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline SHOT352 () = default; + inline constexpr SHOT352 () = default; friend std::ostream& operator << (std::ostream& os, const SHOT352& p); }; @@ -1501,7 +1366,7 @@ namespace pcl float rf[9] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline SHOT1344 () = default; + inline constexpr SHOT1344 () = default; friend std::ostream& operator << (std::ostream& os, const SHOT1344& p); }; @@ -1538,19 +1403,23 @@ namespace pcl PCL_EXPORTS std::ostream& operator << (std::ostream& os, const ReferenceFrame& p); struct EIGEN_ALIGN16 ReferenceFrame : public _ReferenceFrame { - inline ReferenceFrame (const _ReferenceFrame &p) + inline constexpr ReferenceFrame (const _ReferenceFrame &p) : + ReferenceFrame{p.rf} { - std::copy_n(p.rf, 9, rf); + //std::copy_n(p.rf, 9, rf); // this algorithm is constexpr starting from C++20 } - inline ReferenceFrame () + inline constexpr ReferenceFrame () : + _ReferenceFrame{ {{0.0f}} } { - std::fill_n(x_axis, 3, 0.f); + // this algorithm is constexpr starting from C++20 + /*std::fill_n(x_axis, 3, 0.f); std::fill_n(y_axis, 3, 0.f); - std::fill_n(z_axis, 3, 0.f); + std::fill_n(z_axis, 3, 0.f);*/ } - // @TODO: add other ctors + inline constexpr ReferenceFrame (const float (&_rf)[9]) : + _ReferenceFrame{ {{_rf[0], _rf[1], _rf[2], _rf[3], _rf[4], _rf[5], _rf[6], _rf[7], _rf[8]}} } {} friend std::ostream& operator << (std::ostream& os, const ReferenceFrame& p); PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -1566,7 +1435,7 @@ namespace pcl float histogram[33] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline FPFHSignature33 () = default; + inline constexpr FPFHSignature33 () = default; friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p); }; @@ -1580,7 +1449,7 @@ namespace pcl float histogram[308] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline VFHSignature308 () = default; + inline constexpr VFHSignature308 () = default; friend std::ostream& operator << (std::ostream& os, const VFHSignature308& p); }; @@ -1594,7 +1463,7 @@ namespace pcl float histogram[21] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline GRSDSignature21 () = default; + inline constexpr GRSDSignature21 () = default; friend std::ostream& operator << (std::ostream& os, const GRSDSignature21& p); }; @@ -1610,9 +1479,9 @@ namespace pcl unsigned char descriptor[64] = {0}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline BRISKSignature512 () = default; + inline constexpr BRISKSignature512 () = default; - inline BRISKSignature512 (float _scale, float _orientation): scale (_scale), orientation (_orientation) {} + inline constexpr BRISKSignature512 (float _scale, float _orientation): scale (_scale), orientation (_orientation) {} friend std::ostream& operator << (std::ostream& os, const BRISKSignature512& p); }; @@ -1626,7 +1495,7 @@ namespace pcl float histogram[640] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline ESFSignature640 () = default; + inline constexpr ESFSignature640 () = default; friend std::ostream& operator << (std::ostream& os, const ESFSignature640& p); }; @@ -1640,7 +1509,7 @@ namespace pcl float histogram[512] = {0.f}; static constexpr int descriptorSize() { return detail::traits::descriptorSize_v; } - inline GASDSignature512 () = default; + inline constexpr GASDSignature512 () = default; friend std::ostream& operator << (std::ostream& os, const GASDSignature512& p); }; @@ -1654,7 +1523,7 @@ namespace pcl float histogram[984] = {0.f}; static constexpr int descriptorSize() { return detail::traits::descriptorSize_v; } - inline GASDSignature984 () = default; + inline constexpr GASDSignature984 () = default; friend std::ostream& operator << (std::ostream& os, const GASDSignature984& p); }; @@ -1668,7 +1537,7 @@ namespace pcl float histogram[7992] = {0.f}; static constexpr int descriptorSize() { return detail::traits::descriptorSize_v; } - inline GASDSignature7992 () = default; + inline constexpr GASDSignature7992 () = default; friend std::ostream& operator << (std::ostream& os, const GASDSignature7992& p); }; @@ -1682,7 +1551,7 @@ namespace pcl float histogram[16] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline GFPFHSignature16 () = default; + inline constexpr GFPFHSignature16 () = default; friend std::ostream& operator << (std::ostream& os, const GFPFHSignature16& p); }; @@ -1697,11 +1566,11 @@ namespace pcl float descriptor[36] = {0.f}; static constexpr int descriptorSize () { return detail::traits::descriptorSize_v; } - inline Narf36 () = default; + inline constexpr Narf36 () = default; - inline Narf36 (float _x, float _y, float _z): Narf36 (_x, _y, _z, 0.f, 0.f, 0.f) {} + inline constexpr Narf36 (float _x, float _y, float _z): Narf36 (_x, _y, _z, 0.f, 0.f, 0.f) {} - inline Narf36 (float _x, float _y, float _z, float _roll, float _pitch, float _yaw): + inline constexpr Narf36 (float _x, float _y, float _z, float _roll, float _pitch, float _yaw): x (_x), y (_y), z (_z), roll (_roll), pitch (_pitch), yaw (_yaw) {} friend std::ostream& operator << (std::ostream& os, const Narf36& p); @@ -1717,9 +1586,9 @@ namespace pcl BorderTraits traits; //std::vector neighbors; - inline BorderDescription () = default; + inline constexpr BorderDescription () = default; - // TODO: provide other ctors + inline constexpr BorderDescription (int _x, int _y) : x(_x), y(_y) {} friend std::ostream& operator << (std::ostream& os, const BorderDescription& p); }; @@ -1742,9 +1611,9 @@ namespace pcl }; }; - inline IntensityGradient (): IntensityGradient (0.f, 0.f, 0.f) {} + inline constexpr IntensityGradient (): IntensityGradient (0.f, 0.f, 0.f) {} - inline IntensityGradient (float _x, float _y, float _z): gradient_x (_x), gradient_y (_y), gradient_z (_z) {} + inline constexpr IntensityGradient (float _x, float _y, float _z): gradient_x (_x), gradient_y (_y), gradient_z (_z) {} friend std::ostream& operator << (std::ostream& os, const IntensityGradient& p); }; @@ -1787,28 +1656,15 @@ namespace pcl */ struct PointWithScale : public _PointWithScale { - inline PointWithScale (const _PointWithScale &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - scale = p.scale; - angle = p.angle; - response = p.response; - octave = p.octave; - } - - inline PointWithScale (): PointWithScale (0.f, 0.f, 0.f) {} + inline constexpr PointWithScale (const _PointWithScale &p) : + PointWithScale{p.x, p.y, p.z, p.scale, p.angle, p.response, p.octave} {} - inline PointWithScale (float _x, float _y, float _z, float _scale = 1.f, - float _angle = -1.f, float _response = 0.f, int _octave = 0) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - scale = _scale; - angle = _angle; - response = _response; - octave = _octave; - } + inline constexpr PointWithScale (): PointWithScale (0.f, 0.f, 0.f) {} + inline constexpr PointWithScale (float _x, float _y, float _z, float _scale = 1.f, + float _angle = -1.f, float _response = 0.f, int _octave = 0) : + _PointWithScale{{{_x, _y, _z, 1.0f}}, {_scale}, _angle, _response, _octave } {} + friend std::ostream& operator << (std::ostream& os, const PointWithScale& p); }; @@ -1838,26 +1694,21 @@ namespace pcl */ struct PointSurfel : public _PointSurfel { - inline PointSurfel (const _PointSurfel &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - rgba = p.rgba; - radius = p.radius; - confidence = p.confidence; - curvature = p.curvature; - } + inline constexpr PointSurfel (const _PointSurfel &p) : + PointSurfel{p.x, p.y, p.z, p.normal_x, p.normal_y, p.normal_z, p.r, p.g, p.b, p.a, p.radius, p.confidence, p.curvature} {} - inline PointSurfel () - { - x = y = z = 0.0f; - data[3] = 1.0f; - normal_x = normal_y = normal_z = data_n[3] = 0.0f; - r = g = b = 0; - a = 255; - radius = confidence = curvature = 0.0f; - } + inline constexpr PointSurfel () : + PointSurfel{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0u, 0u, 0u, 0u, 0.0f, 0.0f, 0.0f} {} + + inline constexpr PointSurfel (float _x, float _y, float _z, float _nx, + float _ny, float _nz, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, std::uint8_t _a, + float _radius, float _confidence, float _curvature) : + _PointSurfel{ + {{_x, _y, _z, 1.0f}}, + {{_nx, _ny, _nz, 0.0f}}, + {{{{{_b, _g, _r, _a}}}, _radius, _confidence, _curvature}} + } {} - // TODO: add other ctor to PointSurfel friend std::ostream& operator << (std::ostream& os, const PointSurfel& p); }; @@ -1877,28 +1728,17 @@ namespace pcl */ struct PointDEM : public _PointDEM { - inline PointDEM (const _PointDEM &p) - { - x = p.x; y = p.y; z = p.z; data[3] = 1.0f; - intensity = p.intensity; - intensity_variance = p.intensity_variance; - height_variance = p.height_variance; - } - - inline PointDEM (): PointDEM (0.f, 0.f, 0.f) {} - - inline PointDEM (float _x, float _y, float _z): PointDEM (_x, _y, _z, 0.f, 0.f, 0.f) {} + inline constexpr PointDEM (const _PointDEM &p) : + PointDEM{p.x, p.y, p.z, p.intensity, p.intensity_variance, p.height_variance} {} + + inline constexpr PointDEM (): PointDEM (0.f, 0.f, 0.f) {} - inline PointDEM (float _x, float _y, float _z, float _intensity, - float _intensity_variance, float _height_variance) - { - x = _x; y = _y; z = _z; - data[3] = 1.0f; - intensity = _intensity; - intensity_variance = _intensity_variance; - height_variance = _height_variance; - } + inline constexpr PointDEM (float _x, float _y, float _z): PointDEM (_x, _y, _z, 0.f, 0.f, 0.f) {} + inline constexpr PointDEM (float _x, float _y, float _z, float _intensity, + float _intensity_variance, float _height_variance) : + _PointDEM{{{_x, _y, _z, 1.0f}}, _intensity, _intensity_variance, _height_variance} {} + friend std::ostream& operator << (std::ostream& os, const PointDEM& p); }; diff --git a/common/include/pcl/make_shared.h b/common/include/pcl/make_shared.h index 488ec319..983a63c9 100644 --- a/common/include/pcl/make_shared.h +++ b/common/include/pcl/make_shared.h @@ -36,7 +36,7 @@ #pragma once -PCL_DEPRECATED_HEADER(1, 13, "Use instead.") +PCL_DEPRECATED_HEADER(1, 15, "Use instead.") #include diff --git a/common/include/pcl/pcl_macros.h b/common/include/pcl/pcl_macros.h index a86497da..298c58b3 100644 --- a/common/include/pcl/pcl_macros.h +++ b/common/include/pcl/pcl_macros.h @@ -108,7 +108,7 @@ // Macro for emitting pragma warning for deprecated headers #if (defined (__GNUC__) || defined(__clang__)) - #define _PCL_DEPRECATED_HEADER_IMPL(Message) PCL_PRAGMA (GCC warning Message) + #define _PCL_DEPRECATED_HEADER_IMPL(Message) PCL_PRAGMA (message Message) #elif _MSC_VER #define _PCL_DEPRECATED_HEADER_IMPL(Message) PCL_PRAGMA (warning (Message)) #else @@ -448,3 +448,34 @@ aligned_free(void* ptr) #else #define PCL_IF_CONSTEXPR(x) if (x) #endif + +// [[unlikely]] can be used on any conditional branch, but __builtin_expect is restricted to the evaluation point +// This makes it quite difficult to create a single macro for switch and while/if +/** + * @def PCL_CONDITION_UNLIKELY + * @brief Tries to inform the compiler to optimize codegen assuming that the condition will probably evaluate to false + * @note Prefer using `PCL_{IF,WHILE}_UNLIKELY` + * @warning This can't be used with switch statements + * @details This tries to help the compiler optimize for the unlikely case. + * Most compilers assume that the condition would evaluate to true in if and while loops (reference needed) + * As such the opposite of this macro (PCL_CONDITION_LIKELY) will not result in significant performance improvement + * + * Some sample usage: + * @code{.cpp} + * if PCL_CONDITION_UNLIKELY(x == 0) { return; } else { throw std::runtime_error("some error"); } + * // + * while PCL_CONDITION_UNLIKELY(wait_for_result) { sleep(1); } // busy wait, with minimal chances of waiting + * @endcode + */ +#if __has_cpp_attribute(unlikely) + #define PCL_CONDITION_UNLIKELY(x) (static_cast(x)) [[unlikely]] +#elif defined(__GNUC__) + #define PCL_CONDITION_UNLIKELY(x) (__builtin_expect(static_cast(x), 0)) +#elif defined(__clang__) && (PCL_LINEAR_VERSION(__clang_major__, __clang_minor__, 0) >= PCL_LINEAR_VERSION(3, 9, 0)) + #define PCL_CONDITION_UNLIKELY(x) (__builtin_expect(static_cast(x), 0)) +#else // MSVC has no such alternative + #define PCL_CONDITION_UNLIKELY(x) (x) +#endif + +#define PCL_IF_UNLIKELY(x) if PCL_CONDITION_UNLIKELY(x) +#define PCL_WHILE_UNLIKELY(x) while PCL_CONDITION_UNLIKELY(x) diff --git a/common/include/pcl/pcl_tests.h b/common/include/pcl/pcl_tests.h index 2b7f5e4b..7cdb9b57 100644 --- a/common/include/pcl/pcl_tests.h +++ b/common/include/pcl/pcl_tests.h @@ -64,7 +64,7 @@ namespace pcl { SCOPED_TRACE("EXPECT_EQ_VECTORS failed"); EXPECT_EQ (v1.size (), v2.size ()); - std::size_t length = v1.size (); + std::size_t length = std::min (v1.size (), v2.size ()); for (std::size_t i = 0; i < length; ++i) EXPECT_EQ (v1[i], v2[i]); } @@ -74,7 +74,7 @@ namespace pcl { SCOPED_TRACE("EXPECT_NEAR_VECTORS failed"); EXPECT_EQ (v1.size (), v2.size ()); - std::size_t length = v1.size (); + std::size_t length = std::min (v1.size (), v2.size()); for (std::size_t i = 0; i < length; ++i) EXPECT_NEAR (v1[i], v2[i], epsilon); } diff --git a/common/include/pcl/point_cloud.h b/common/include/pcl/point_cloud.h index 9440c922..fa2172df 100644 --- a/common/include/pcl/point_cloud.h +++ b/common/include/pcl/point_cloud.h @@ -595,6 +595,12 @@ namespace pcl inline void assign(InputIterator first, InputIterator last, index_t new_width) { + if (new_width == 0) { + PCL_WARN("Assignment with new_width equal to 0," + "setting width to size of the cloud and height to 1\n"); + return assign(std::move(first), std::move(last)); + } + points.assign(std::move(first), std::move(last)); width = new_width; height = size() / width; @@ -631,6 +637,11 @@ namespace pcl void inline assign(std::initializer_list ilist, index_t new_width) { + if (new_width == 0) { + PCL_WARN("Assignment with new_width equal to 0," + "setting width to size of the cloud and height to 1\n"); + return assign(std::move(ilist)); + } points.assign(std::move(ilist)); width = new_width; height = size() / width; diff --git a/common/include/pcl/point_representation.h b/common/include/pcl/point_representation.h index 4bcabcfd..5acdd9eb 100644 --- a/common/include/pcl/point_representation.h +++ b/common/include/pcl/point_representation.h @@ -165,7 +165,7 @@ namespace pcl setRescaleValues (const float *rescale_array) { alpha_.resize (nr_dimensions_); - std::copy_n(rescale_array, nr_dimensions_, alpha_.begin()); + std::copy(rescale_array, rescale_array + nr_dimensions_, alpha_.begin()); } /** \brief Return the number of dimensions in the point's vector representation. */ @@ -196,7 +196,7 @@ namespace pcl trivial_ = true; } - ~DefaultPointRepresentation () {} + ~DefaultPointRepresentation () override = default; inline Ptr makeShared () const @@ -209,7 +209,7 @@ namespace pcl { // If point type is unknown, treat it as a struct/array of floats const float* ptr = reinterpret_cast (&p); - std::copy_n(ptr, nr_dimensions_, out); + std::copy(ptr, ptr + nr_dimensions_, out); } }; @@ -563,12 +563,12 @@ namespace pcl * \param[in] p the input point * \param[out] out the resultant output array */ - virtual void - copyToFloatArray (const PointDefault &p, float *out) const + void + copyToFloatArray (const PointDefault &p, float *out) const override { // If point type is unknown, treat it as a struct/array of floats const float *ptr = (reinterpret_cast (&p)) + start_dim_; - std::copy_n(ptr, nr_dimensions_, out); + std::copy(ptr, ptr + nr_dimensions_, out); } protected: diff --git a/common/include/pcl/point_traits.h b/common/include/pcl/point_traits.h index f29592a5..4136fa91 100644 --- a/common/include/pcl/point_traits.h +++ b/common/include/pcl/point_traits.h @@ -38,6 +38,6 @@ #pragma once -PCL_DEPRECATED_HEADER(1, 13, "Use instead.") +PCL_DEPRECATED_HEADER(1, 15, "Use instead.") #include diff --git a/common/include/pcl/point_types_conversion.h b/common/include/pcl/point_types_conversion.h index 231b5f20..a4afdace 100644 --- a/common/include/pcl/point_types_conversion.h +++ b/common/include/pcl/point_types_conversion.h @@ -303,7 +303,7 @@ namespace pcl { out.width = in.width; out.height = in.height; - for (const auto &point : in.points) + for (const auto &point : in) { Intensity p; PointRGBtoI (point, p); @@ -321,7 +321,7 @@ namespace pcl { out.width = in.width; out.height = in.height; - for (const auto &point : in.points) + for (const auto &point : in) { Intensity8u p; PointRGBtoI (point, p); @@ -339,7 +339,7 @@ namespace pcl { out.width = in.width; out.height = in.height; - for (const auto &point : in.points) + for (const auto &point : in) { Intensity32u p; PointRGBtoI (point, p); @@ -357,13 +357,31 @@ namespace pcl { out.width = in.width; out.height = in.height; - for (const auto &point : in.points) + for (const auto &point : in) { PointXYZHSV p; PointXYZRGBtoXYZHSV (point, p); out.push_back (p); } } + + /** \brief Convert a XYZHSV point cloud to a XYZRGB + * \param[in] in the input XYZHSV point cloud + * \param[out] out the output XYZRGB point cloud + */ + inline void + PointCloudXYZHSVtoXYZRGB (const PointCloud& in, + PointCloud& out) + { + out.width = in.width; + out.height = in.height; + for (const auto &point : in) + { + PointXYZRGB p; + PointXYZHSVtoXYZRGB (point, p); + out.push_back (p); + } + } /** \brief Convert a XYZRGB point cloud to a XYZHSV * \param[in] in the input XYZRGB point cloud @@ -375,7 +393,7 @@ namespace pcl { out.width = in.width; out.height = in.height; - for (const auto &point : in.points) + for (const auto &point : in) { PointXYZHSV p; PointXYZRGBAtoXYZHSV (point, p); @@ -393,7 +411,7 @@ namespace pcl { out.width = in.width; out.height = in.height; - for (const auto &point : in.points) + for (const auto &point : in) { PointXYZI p; PointXYZRGBtoXYZI (point, p); diff --git a/common/include/pcl/range_image/impl/range_image.hpp b/common/include/pcl/range_image/impl/range_image.hpp index 62cfe2ae..5731b4cc 100644 --- a/common/include/pcl/range_image/impl/range_image.hpp +++ b/common/include/pcl/range_image/impl/range_image.hpp @@ -44,6 +44,7 @@ #include #include // for pcl::isFinite #include // for VectorAverage3f +#include namespace pcl { @@ -135,9 +136,11 @@ RangeImage::createFromPointCloud (const PointCloudType& point_cloud, int top=height, right=-1, bottom=-1, left=width; doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left); - cropImage (border_size, top, right, bottom, left); + if (border_size != std::numeric_limits::min()) { + cropImage (border_size, top, right, bottom, left); - recalculate3DPointPositions (); + recalculate3DPointPositions (); + } } ///////////////////////////////////////////////////////////////////////// @@ -196,9 +199,11 @@ RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud int top=height, right=-1, bottom=-1, left=width; doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left); - cropImage (border_size, top, right, bottom, left); + if (border_size != std::numeric_limits::min()) { + cropImage (border_size, top, right, bottom, left); - recalculate3DPointPositions (); + recalculate3DPointPositions (); + } } ///////////////////////////////////////////////////////////////////////// @@ -236,9 +241,8 @@ RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, flo const typename pcl::PointCloud::VectorType &points2 = point_cloud.points; unsigned int size = width*height; - int* counters = new int[size]; - ERASE_ARRAY (counters, size); - + std::vector counters(size, 0); + top=height; right=-1; bottom=-1; left=width; float x_real, y_real, range_of_current_point; @@ -276,7 +280,7 @@ RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, flo if (isInImage (n_x, n_y)) { int neighbor_array_pos = n_y*width + n_x; - if (counters[neighbor_array_pos]==0) + if (counters[neighbor_array_pos] == 0) { float& neighbor_range = points[neighbor_array_pos].range; neighbor_range = (std::isinf (neighbor_range) ? range_of_current_point : (std::min) (neighbor_range, range_of_current_point)); @@ -321,8 +325,6 @@ RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, flo range_at_image_point += (range_of_current_point-range_at_image_point)/counter; } } - - delete[] counters; } ///////////////////////////////////////////////////////////////////////// @@ -356,6 +358,11 @@ RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& { Eigen::Vector3f transformedPoint = to_range_image_system_ * point; range = transformedPoint.norm (); + if (range < std::numeric_limits::epsilon()) { + PCL_DEBUG ("[pcl::RangeImage::getImagePoint] Transformed point is (0,0,0), cannot project it.\n"); + image_x = image_y = 0.0f; + return; + } float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]), angle_y = asinLookUp (transformedPoint[1]/range); getImagePointFromAngles (angle_x, angle_y, image_x, image_y); diff --git a/common/include/pcl/range_image/range_image.h b/common/include/pcl/range_image/range_image.h index 4405ed0d..782516d5 100644 --- a/common/include/pcl/range_image/range_image.h +++ b/common/include/pcl/range_image/range_image.h @@ -139,7 +139,7 @@ namespace pcl * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and * will always take the minimum per cell. * \param min_range the minimum visible range (defaults to 0) - * \param border_size the border size (defaults to 0) + * \param border_size the border size (defaults to 0). Set to `std::numeric_limits::min()` to turn cropping off. */ template void createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution=pcl::deg2rad (0.5f), @@ -163,7 +163,7 @@ namespace pcl * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and * will always take the minimum per cell. * \param min_range the minimum visible range (defaults to 0) - * \param border_size the border size (defaults to 0) + * \param border_size the border size (defaults to 0). Set to `std::numeric_limits::min()` to turn cropping off. */ template void createFromPointCloud (const PointCloudType& point_cloud, @@ -186,7 +186,7 @@ namespace pcl * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and * will always take the minimum per cell. * \param min_range the minimum visible range (defaults to 0) - * \param border_size the border size (defaults to 0) + * \param border_size the border size (defaults to 0). Set to `std::numeric_limits::min()` to turn cropping off. */ template void createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, @@ -211,7 +211,7 @@ namespace pcl * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and * will always take the minimum per cell. * \param min_range the minimum visible range (defaults to 0) - * \param border_size the border size (defaults to 0) + * \param border_size the border size (defaults to 0). Set to `std::numeric_limits::min()` to turn cropping off. */ template void createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, @@ -232,7 +232,7 @@ namespace pcl * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and * will always take the minimum per cell. * \param min_range the minimum visible range (defaults to 0) - * \param border_size the border size (defaults to 0) + * \param border_size the border size (defaults to 0). Set to `std::numeric_limits::min()` to turn cropping off. * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y * to the bottom and z to the front) */ @@ -256,7 +256,7 @@ namespace pcl * but the mean of the points. If 0.0 it is equivalent to a normal z-buffer and * will always take the minimum per cell. * \param min_range the minimum visible range (defaults to 0) - * \param border_size the border size (defaults to 0) + * \param border_size the border size (defaults to 0). Set to `std::numeric_limits::min()` to turn cropping off. * \note If wrong_coordinate_system is true, the sensor pose will be rotated to change from a coordinate frame * with x to the front, y to the left and z to the top to the coordinate frame we use here (x to the right, y * to the bottom and z to the front) */ diff --git a/common/include/pcl/range_image/range_image_planar.h b/common/include/pcl/range_image/range_image_planar.h index a0951188..3d2cac07 100644 --- a/common/include/pcl/range_image/range_image_planar.h +++ b/common/include/pcl/range_image/range_image_planar.h @@ -60,7 +60,7 @@ namespace pcl /** Constructor */ PCL_EXPORTS RangeImagePlanar (); /** Destructor */ - PCL_EXPORTS ~RangeImagePlanar (); + PCL_EXPORTS ~RangeImagePlanar () override; /** Return a newly created RangeImagePlanar. * Reimplementation to return an image of the same type. */ diff --git a/common/include/pcl/range_image/range_image_spherical.h b/common/include/pcl/range_image/range_image_spherical.h index c2ffc601..2a80fa5d 100644 --- a/common/include/pcl/range_image/range_image_spherical.h +++ b/common/include/pcl/range_image/range_image_spherical.h @@ -59,7 +59,7 @@ namespace pcl /** Constructor */ PCL_EXPORTS RangeImageSpherical () {} /** Destructor */ - PCL_EXPORTS virtual ~RangeImageSpherical () {} + PCL_EXPORTS virtual ~RangeImageSpherical () = default; /** Return a newly created RangeImagePlanar. * Reimplementation to return an image of the same type. */ diff --git a/common/include/pcl/register_point_struct.h b/common/include/pcl/register_point_struct.h index b3d36ff2..1a09228b 100644 --- a/common/include/pcl/register_point_struct.h +++ b/common/include/pcl/register_point_struct.h @@ -189,6 +189,23 @@ namespace pcl for (std::uint32_t i = 0; i < count; ++i) p[i] /= scalar; } + + template inline + std::enable_if_t::value> + divscalar2 (NoArrayT &p, const ScalarT &scalar) + { + p = scalar / p; + } + + template inline + std::enable_if_t::value> + divscalar2 (ArrayT &p, const ScalarT &scalar) + { + using type = std::remove_all_extents_t; + static const std::uint32_t count = sizeof (ArrayT) / sizeof (type); + for (std::uint32_t i = 0; i < count; ++i) + p[i] = scalar / p[i]; + } } } @@ -225,6 +242,11 @@ namespace pcl scalar); /***/ +#define PCL_DIVEQSC2_POINT_TAG(r, data, elem) \ + pcl::traits::divscalar2 (p.BOOST_PP_TUPLE_ELEM(3, 1, elem), \ + scalar); + /***/ + // Construct type traits given full sequence of (type, name, tag) triples // BOOST_MPL_ASSERT_MSG(std::is_pod::value, // REGISTERED_POINT_TYPE_MUST_BE_PLAIN_OLD_DATA, (name)); @@ -263,6 +285,16 @@ namespace pcl inline const name operator+ (const name& p, const float& scalar) \ { name result = p; result += scalar; return (result); } \ inline const name& \ + operator*= (name& p, const float& scalar) \ + { \ + BOOST_PP_SEQ_FOR_EACH(PCL_MULEQSC_POINT_TAG, _, seq) \ + return (p); \ + } \ + inline const name operator* (const float& scalar, const name& p) \ + { name result = p; result *= scalar; return (result); } \ + inline const name operator* (const name& p, const float& scalar) \ + { name result = p; result *= scalar; return (result); } \ + inline const name& \ operator-= (name& lhs, const name& rhs) \ { \ BOOST_PP_SEQ_FOR_EACH(PCL_MINUSEQ_POINT_TAG, _, seq) \ @@ -277,27 +309,18 @@ namespace pcl inline const name operator- (const name& lhs, const name& rhs) \ { name result = lhs; result -= rhs; return (result); } \ inline const name operator- (const float& scalar, const name& p) \ - { name result = p; result -= scalar; return (result); } \ + { name result = p; result *= -1.0f; result += scalar; return (result); } \ inline const name operator- (const name& p, const float& scalar) \ { name result = p; result -= scalar; return (result); } \ inline const name& \ - operator*= (name& p, const float& scalar) \ - { \ - BOOST_PP_SEQ_FOR_EACH(PCL_MULEQSC_POINT_TAG, _, seq) \ - return (p); \ - } \ - inline const name operator* (const float& scalar, const name& p) \ - { name result = p; result *= scalar; return (result); } \ - inline const name operator* (const name& p, const float& scalar) \ - { name result = p; result *= scalar; return (result); } \ - inline const name& \ operator/= (name& p, const float& scalar) \ { \ BOOST_PP_SEQ_FOR_EACH(PCL_DIVEQSC_POINT_TAG, _, seq) \ return (p); \ } \ - inline const name operator/ (const float& scalar, const name& p) \ - { name result = p; result /= scalar; return (result); } \ + inline const name operator/ (const float& scalar, const name& p_in) \ + { name p = p_in; BOOST_PP_SEQ_FOR_EACH(PCL_DIVEQSC2_POINT_TAG, _, seq) \ + return (p); } \ inline const name operator/ (const name& p, const float& scalar) \ { name result = p; result /= scalar; return (result); } \ } \ diff --git a/common/include/pcl/type_traits.h b/common/include/pcl/type_traits.h index d9c6b40d..16559a89 100644 --- a/common/include/pcl/type_traits.h +++ b/common/include/pcl/type_traits.h @@ -73,32 +73,40 @@ namespace pcl static const std::uint8_t INT8 = 1, UINT8 = 2, INT16 = 3, UINT16 = 4, INT32 = 5, UINT32 = 6, - FLOAT32 = 7, FLOAT64 = 8; + FLOAT32 = 7, FLOAT64 = 8, + INT64 = 9, UINT64 = 10, + BOOL = 11; }; - } + } // namespace detail // Metafunction to return enum value representing a type template struct asEnum {}; + template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::BOOL; }; template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::INT8; }; template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::UINT8; }; template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::INT16; }; template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::UINT16; }; template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::INT32; }; template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::UINT32; }; - template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::FLOAT32; }; - template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::FLOAT64; }; + template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::INT64; }; + template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::UINT64; }; + template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::FLOAT32; }; + template<> struct asEnum { static const std::uint8_t value = detail::PointFieldTypes::FLOAT64; }; template static constexpr std::uint8_t asEnum_v = asEnum::value; // Metafunction to return type of enum value template struct asType {}; + template<> struct asType { using type = bool; }; template<> struct asType { using type = std::int8_t; }; template<> struct asType { using type = std::uint8_t; }; template<> struct asType { using type = std::int16_t; }; template<> struct asType { using type = std::uint16_t; }; template<> struct asType { using type = std::int32_t; }; template<> struct asType { using type = std::uint32_t; }; + template<> struct asType { using type = std::int64_t; }; + template<> struct asType { using type = std::uint64_t; }; template<> struct asType { using type = float; }; template<> struct asType { using type = double; }; diff --git a/common/src/feature_histogram.cpp b/common/src/feature_histogram.cpp index cad1d39f..12b9711a 100644 --- a/common/src/feature_histogram.cpp +++ b/common/src/feature_histogram.cpp @@ -64,10 +64,7 @@ pcl::FeatureHistogram::FeatureHistogram (std::size_t const number_of_bins, number_of_bins_ = number_of_bins; } -pcl::FeatureHistogram::~FeatureHistogram () -{ - -} +pcl::FeatureHistogram::~FeatureHistogram () = default; float pcl::FeatureHistogram::getThresholdMin () const @@ -103,7 +100,7 @@ pcl::FeatureHistogram::addValue (float value) ++number_of_elements_; // Increase the bin. - std::size_t bin_number = static_cast ((value - threshold_min_) / step_); + auto bin_number = static_cast ((value - threshold_min_) / step_); ++histogram_[bin_number]; } } diff --git a/common/src/fft/kiss_fft.c b/common/src/fft/kiss_fft.c index a48514a0..1112589f 100644 --- a/common/src/fft/kiss_fft.c +++ b/common/src/fft/kiss_fft.c @@ -260,11 +260,13 @@ void kf_work( #if (defined _OPENMP && (_OPENMP <= 201307)) || (defined __GNUC__ && (__GNUC__ >= 6 && __GNUC__ < 9)) #pragma omp parallel for \ default(none) \ - shared(f, factors, Fout, in_stride) + shared(f, factors, Fout, in_stride) \ + private(k) #else #pragma omp parallel for \ default(none) \ - shared(f, factors, Fout, fstride, in_stride, m, p, st) + shared(f, factors, Fout, fstride, in_stride, m, p, st) \ + private(k) #endif for (k=0;k 0) - memset (&cloud_out.data[point_offset + field_offset], 0, padding_size); + if (padding_size > 0) { + std::fill_n(&cloud_out.data[point_offset + field_offset], padding_size, 0); + } field_offset += padding_size; } point_offset += field_offset; diff --git a/common/src/parse.cpp b/common/src/parse.cpp index 1d8d9d63..26ff83fc 100644 --- a/common/src/parse.cpp +++ b/common/src/parse.cpp @@ -120,7 +120,7 @@ parse_argument (int argc, const char * const * argv, const char * str, long long int parse_argument (int argc, const char * const * argv, const char * str, unsigned long long int &val) noexcept { - long long int dummy; + long long int dummy = -1; const auto ret = parse_argument (argc, argv, str, dummy); if ((ret == -1) || dummy < 0) { diff --git a/common/src/pcl_base.cpp b/common/src/pcl_base.cpp index e591812d..4b3f18da 100644 --- a/common/src/pcl_base.cpp +++ b/common/src/pcl_base.cpp @@ -40,6 +40,7 @@ #include #include +#include // for getFieldSize /////////////////////////////////////////////////////////////////////////////////////////// pcl::PCLBase::PCLBase () @@ -74,32 +75,19 @@ pcl::PCLBase::setInputCloud (const PCLPointCloud2ConstPtr & // Obtain the size of datatype const auto sizeofDatatype = [](const auto& datatype) -> int { - switch (datatype) - { - case pcl::PCLPointField::INT8: - case pcl::PCLPointField::UINT8: return 1; - - case pcl::PCLPointField::INT16: - case pcl::PCLPointField::UINT16: return 2; - - case pcl::PCLPointField::INT32: - case pcl::PCLPointField::UINT32: - case pcl::PCLPointField::FLOAT32: return 4; - - case pcl::PCLPointField::FLOAT64: return 8; - - default: + const auto size = getFieldSize(datatype); + if (size == 0) { PCL_ERROR("[PCLBase::setInputCloud] Invalid field type (%d)!\n", datatype); - return 0; } + return size; }; - // Restrict size of a field to be at-max sizeof(FLOAT32) for now + // Restrict size of a field to be at-max sizeof(FLOAT64) now to support {U}INT64 field_sizes_.resize(input_->fields.size()); std::transform(input_->fields.begin(), input_->fields.end(), field_sizes_.begin(), [&sizeofDatatype](const auto& field) { - return std::min(sizeofDatatype(field.datatype), static_cast(sizeof(float))); + return std::min(sizeofDatatype(field.datatype), static_cast(sizeof(double))); }); } diff --git a/common/src/projection_matrix.cpp b/common/src/projection_matrix.cpp index c4fb7cdb..2239a929 100644 --- a/common/src/projection_matrix.cpp +++ b/common/src/projection_matrix.cpp @@ -49,7 +49,7 @@ pcl::getCameraMatrixFromProjectionMatrix ( Eigen::Matrix3f cam = KR_KRT / KR_KRT.coeff (8); - memset (&(camera_matrix.coeffRef (0)), 0, sizeof (Eigen::Matrix3f::Scalar) * 9); + camera_matrix.setZero(); camera_matrix.coeffRef (8) = 1.0; if (camera_matrix.Flags & Eigen::RowMajorBit) diff --git a/common/src/range_image.cpp b/common/src/range_image.cpp index 48b0a6a1..342bad4e 100644 --- a/common/src/range_image.cpp +++ b/common/src/range_image.cpp @@ -39,6 +39,8 @@ #include // for MEASURE_FUNCTION_TIME #include +#include + namespace pcl { @@ -466,8 +468,8 @@ RangeImage::getInterpolatedSurfaceProjection (const Eigen::Affine3f& pose, int p int no_of_pixels = pixel_size*pixel_size; float* surface_patch = new float[no_of_pixels]; - SET_ARRAY (surface_patch, -std::numeric_limits::infinity (), no_of_pixels); - + std::fill_n(surface_patch, no_of_pixels, -std::numeric_limits::infinity ()); + Eigen::Vector3f position = inverse_pose.translation (); int middle_x, middle_y; getImagePoint (position, middle_x, middle_y); diff --git a/common/src/range_image_planar.cpp b/common/src/range_image_planar.cpp index 5c649aea..97b249bc 100644 --- a/common/src/range_image_planar.cpp +++ b/common/src/range_image_planar.cpp @@ -50,9 +50,7 @@ namespace pcl } ///////////////////////////////////////////////////////////////////////// - RangeImagePlanar::~RangeImagePlanar () - { - } + RangeImagePlanar::~RangeImagePlanar () = default; ///////////////////////////////////////////////////////////////////////// void @@ -212,7 +210,7 @@ namespace pcl std::cerr << __PRETTY_FUNCTION__<<": Given range image is not a RangeImagePlanar!\n"; return; } - RangeImagePlanar& ret = * (static_cast (&half_image)); + RangeImagePlanar& ret = * (dynamic_cast (&half_image)); ret.focal_length_x_ = focal_length_x_/2; ret.focal_length_x_reciprocal_ = 1.0f/ret.focal_length_x_; @@ -235,7 +233,7 @@ namespace pcl std::cerr << __PRETTY_FUNCTION__<<": Given range image is not a RangeImagePlanar!\n"; return; } - RangeImagePlanar& ret = * (static_cast (&sub_image)); + RangeImagePlanar& ret = * (dynamic_cast (&sub_image)); ret.focal_length_x_ = focal_length_x_ / static_cast (combine_pixels); ret.focal_length_x_reciprocal_ = 1.0f / ret.focal_length_x_; @@ -257,7 +255,7 @@ namespace pcl std::cerr << PVARC(typeid (*this).name())< (&other) = *this; + *dynamic_cast (&other) = *this; } } // namespace end diff --git a/cuda/apps/CMakeLists.txt b/cuda/apps/CMakeLists.txt index e6e519e9..a8190839 100644 --- a/cuda/apps/CMakeLists.txt +++ b/cuda/apps/CMakeLists.txt @@ -15,7 +15,7 @@ endif() PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") mark_as_advanced("BUILD_${SUBSYS_NAME}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) if(NOT build) return() diff --git a/cuda/apps/src/kinect_planes_cuda.cpp b/cuda/apps/src/kinect_planes_cuda.cpp index e344b723..c61c0336 100644 --- a/cuda/apps/src/kinect_planes_cuda.cpp +++ b/cuda/apps/src/kinect_planes_cuda.cpp @@ -183,7 +183,7 @@ class MultiRansac typename Storage::type region_mask; markInliers (data, region_mask, planes); thrust::host_vector regions_host; - std::copy (regions_host.begin (), regions_host.end(), std::ostream_iterator(std::cerr, " ")); + std::copy (regions_host.cbegin (), regions_host.cend(), std::ostream_iterator(std::cerr, " ")); { ScopeTimeCPU t ("retrieving inliers"); planes = sac.getAllInliers (); diff --git a/cuda/apps/src/kinect_segmentation_cuda.cpp b/cuda/apps/src/kinect_segmentation_cuda.cpp index c6b6510b..2b0e242d 100644 --- a/cuda/apps/src/kinect_segmentation_cuda.cpp +++ b/cuda/apps/src/kinect_segmentation_cuda.cpp @@ -281,7 +281,7 @@ class Segmentation typename Storage::type region_mask; markInliers (data, region_mask, planes); thrust::host_vector regions_host; - std::copy (regions_host.begin (), regions_host.end(), std::ostream_iterator(std::cerr, " ")); + std::copy (regions_host.cbegin (), regions_host.cend(), std::ostream_iterator(std::cerr, " ")); { ScopeTimeCPU t ("retrieving inliers"); planes = sac.getAllInliers (); diff --git a/cuda/common/CMakeLists.txt b/cuda/common/CMakeLists.txt index e7a2ff2a..2c4c119a 100644 --- a/cuda/common/CMakeLists.txt +++ b/cuda/common/CMakeLists.txt @@ -6,7 +6,7 @@ set(SUBSYS_DEPS) set(build TRUE) PCL_SUBSYS_OPTION(build ${SUBSYS_NAME} ${SUBSYS_DESC} ON) mark_as_advanced("BUILD_${SUBSYS_NAME}") -PCL_SUBSYS_DEPEND(build ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) PCL_SET_SUBSYS_INCLUDE_DIR(${SUBSYS_NAME} ${SUBSYS_PATH}) ## ---[ Point Cloud Library - pcl/cuda/common diff --git a/cuda/common/include/pcl/cuda/common/eigen.h b/cuda/common/include/pcl/cuda/common/eigen.h index 5ae2f949..0d1eb8dd 100644 --- a/cuda/common/include/pcl/cuda/common/eigen.h +++ b/cuda/common/include/pcl/cuda/common/eigen.h @@ -93,7 +93,6 @@ #include #include -#include namespace pcl { @@ -102,7 +101,9 @@ namespace pcl inline __host__ __device__ bool isMuchSmallerThan (float x, float y) { - float prec_sqr = FLT_EPSILON * FLT_EPSILON; // copied from /include/Eigen/src/Core/NumTraits.h + // inspired by Eigen's implementation + float prec_sqr = + std::numeric_limits::epsilon() * std::numeric_limits::epsilon(); return x * x <= prec_sqr * y * y; } @@ -179,7 +180,7 @@ namespace pcl float c2 = m.data[0].x + m.data[1].y + m.data[2].z; - if (std::abs(c0) < FLT_EPSILON) // one root is 0 -> quadratic equation + if (std::abs(c0) < std::numeric_limits::epsilon()) // one root is 0 -> quadratic equation computeRoots2 (c2, c1, roots); else { @@ -233,7 +234,7 @@ namespace pcl //Scalar scale = mat.cwiseAbs ().maxCoeff (); float3 scale_tmp = fmaxf (fmaxf (fabs (mat.data[0]), fabs (mat.data[1])), fabs (mat.data[2])); float scale = fmaxf (fmaxf (scale_tmp.x, scale_tmp.y), scale_tmp.z); - if (scale <= FLT_MIN) + if (scale <= std::numeric_limits::min()) scale = 1.0f; CovarianceMatrix scaledMat; @@ -244,14 +245,14 @@ namespace pcl // Compute the eigenvalues computeRoots (scaledMat, evals); - if ((evals.z-evals.x) <= FLT_EPSILON) + if ((evals.z - evals.x) <= std::numeric_limits::epsilon()) { // all three equal evecs.data[0] = make_float3 (1.0f, 0.0f, 0.0f); evecs.data[1] = make_float3 (0.0f, 1.0f, 0.0f); evecs.data[2] = make_float3 (0.0f, 0.0f, 1.0f); } - else if ((evals.y-evals.x) <= FLT_EPSILON) + else if ((evals.y - evals.x) <= std::numeric_limits::epsilon()) { // first and second equal CovarianceMatrix tmp; @@ -281,7 +282,7 @@ namespace pcl evecs.data[1] = unitOrthogonal (evecs.data[2]); evecs.data[0] = cross (evecs.data[1], evecs.data[2]); } - else if ((evals.z-evals.y) <= FLT_EPSILON) + else if ((evals.z - evals.y) <= std::numeric_limits::epsilon()) { // second and third equal CovarianceMatrix tmp; diff --git a/cuda/common/include/pcl/cuda/common/point_type_rgb.h b/cuda/common/include/pcl/cuda/common/point_type_rgb.h index 1f63a388..e4d28248 100644 --- a/cuda/common/include/pcl/cuda/common/point_type_rgb.h +++ b/cuda/common/include/pcl/cuda/common/point_type_rgb.h @@ -69,13 +69,11 @@ namespace cuda return (r == rhs.r && g == rhs.g && b == rhs.b && alpha == rhs.alpha); } - inline __host__ __device__ RGB& operator - (RGB &rhs) + inline __host__ __device__ RGB operator - (RGB &rhs) { - r = -r; - g = -g; - b = -b; - alpha = -alpha; - return (*this); + RGB res = *this; + res -= rhs; + return (res); } inline __host__ __device__ RGB& operator += (const RGB &rhs) diff --git a/cuda/features/CMakeLists.txt b/cuda/features/CMakeLists.txt index 3d0d1c30..0932378c 100644 --- a/cuda/features/CMakeLists.txt +++ b/cuda/features/CMakeLists.txt @@ -8,7 +8,7 @@ set(SUBSYS_DEPS cuda_common io common) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) mark_as_advanced("BUILD_${SUBSYS_NAME}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}") if(NOT build) diff --git a/cuda/filters/include/pcl/cuda/filters/filter.h b/cuda/filters/include/pcl/cuda/filters/filter.h index cb63d76c..ae26d836 100644 --- a/cuda/filters/include/pcl/cuda/filters/filter.h +++ b/cuda/filters/include/pcl/cuda/filters/filter.h @@ -36,7 +36,7 @@ #pragma once #include -#include +#include namespace pcl_cuda { @@ -69,7 +69,8 @@ namespace pcl_cuda /** \brief Empty constructor. */ Filter () : filter_field_name_ (""), - filter_limit_min_ (-FLT_MAX), filter_limit_max_ (FLT_MAX), + filter_limit_min_ (std::numeric_limits::lowest()), + filter_limit_max_ (std::numeric_limits::max()), filter_limit_negative_ (false) {}; @@ -98,7 +99,7 @@ namespace pcl_cuda } /** \brief Get the field filter limits (min/max) set by the user. - * The default values are -FLT_MAX, FLT_MAX. + * The default values are std::numeric_limits::lowest(), std::numeric_limits::max(). * \param limit_min the minimum limit * \param limit_max the maximum limit */ diff --git a/cuda/io/CMakeLists.txt b/cuda/io/CMakeLists.txt index f60a472e..8361560b 100644 --- a/cuda/io/CMakeLists.txt +++ b/cuda/io/CMakeLists.txt @@ -9,7 +9,7 @@ set(SUBSYS_EXT_DEPS openni) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF) mark_as_advanced("BUILD_${SUBSYS_NAME}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS ${SUBSYS_EXT_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS} EXT_DEPS ${SUBSYS_EXT_DEPS}) PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}") if(NOT build) diff --git a/cuda/io/src/disparity_to_cloud.cu b/cuda/io/src/disparity_to_cloud.cu index 1c68f065..877f1d63 100644 --- a/cuda/io/src/disparity_to_cloud.cu +++ b/cuda/io/src/disparity_to_cloud.cu @@ -102,8 +102,6 @@ ComputeXYZRGB::operator () (const Tuple &t) // if (!output) // output.reset (new PointCloudAOS); // -// using namespace thrust; -// // // Prepare the output // output->height = depth_image->height; // output->width = depth_image->width; @@ -126,8 +124,8 @@ ComputeXYZRGB::operator () (const Tuple &t) // // // Send the data to the device // transform ( -// make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator(0))), -// make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator(0))) + +// thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), thrust::counting_iterator(0))), +// thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), thrust::counting_iterator(0))) + // depth_image->width * depth_image->height, // output->points.begin (), // ComputeXYZRGB (depth_image->width, depth_image->height, @@ -137,8 +135,8 @@ ComputeXYZRGB::operator () (const Tuple &t) // { // // Send the data to the device // transform ( -// make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))), -// make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))) + +// thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))), +// thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))) + // depth_image->width * depth_image->height, // output->points.begin (), // ComputeXYZ (depth_image->width, depth_image->height, @@ -191,7 +189,6 @@ DisparityToCloud::compute (const std::uint16_t* depth_image, typename PointCloudAOS::Ptr &output, int smoothing_nr_iterations, int smoothing_filter_size) { - using namespace thrust; if (!output) output.reset (new PointCloudAOS); @@ -216,25 +213,25 @@ DisparityToCloud::compute (const std::uint16_t* depth_image, { typename Storage::type disp_helper_map (output_size); - float* depth_ptr = raw_pointer_cast(&depth[0]); + float* depth_ptr = thrust::raw_pointer_cast(&depth[0]); - transform (counting_iterator(0), - counting_iterator(0) + output_size, + transform (thrust::counting_iterator(0), + thrust::counting_iterator(0) + output_size, disp_helper_map.begin (), DisparityHelperMap (depth_ptr, width, height, smoothing_filter_size, baseline, 1.0f/constant, disp_thresh)); for (int iter = 0; iter < smoothing_nr_iterations; iter++) { transform ( - make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))) + output_size, - depth.begin (), DisparityClampedSmoothing (raw_pointer_cast(&depth[0]), raw_pointer_cast(&disp_helper_map[0]), width, height, smoothing_filter_size)); + thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))) + output_size, + depth.begin (), DisparityClampedSmoothing (thrust::raw_pointer_cast(&depth[0]), thrust::raw_pointer_cast(&disp_helper_map[0]), width, height, smoothing_filter_size)); } // Send the data to the device transform ( - make_zip_iterator (make_tuple (depth.begin (), rgb.begin(), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin (), rgb.begin(), counting_iterator(0))) + output_size, + thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin(), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin(), thrust::counting_iterator(0))) + output_size, output->points.begin (), ComputeXYZRGB (width, height, width >> 1, height >> 1, constant)); @@ -242,8 +239,8 @@ DisparityToCloud::compute (const std::uint16_t* depth_image, else { transform ( - make_zip_iterator (make_tuple (depth.begin(), rgb.begin(), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin(), rgb.begin(), counting_iterator(0))) + output_size, + thrust::make_zip_iterator (make_tuple (depth.begin(), rgb.begin(), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin(), rgb.begin(), thrust::counting_iterator(0))) + output_size, output->points.begin (), ComputeXYZRGB (width, height, width >> 1, height >> 1, constant)); @@ -261,8 +258,6 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, if (!output) output.reset (new PointCloudAOS); - using namespace thrust; - int depth_width = depth_image->getWidth (); int depth_height = depth_image->getHeight (); @@ -295,7 +290,7 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, thrust::counting_iterator counter (0); //typename Storage::type downsampled_indices; //downsampled_indices.resize ((output->width/2) * (output->height/2)); - //thrust::copy_if (counting_iterator(0), counting_iterator(0)+depth_width*depth_height, downsampled_indices.begin (), downsampleIndices (output->width, output->height, 2)); + //thrust::copy_if (thrust::counting_iterator(0), thrust::counting_iterator(0)+depth_width*depth_height, downsampled_indices.begin (), downsampleIndices (output->width, output->height, 2)); thrust::copy_if (depth_device.begin (), depth_device.end (), //thrust::make_constant_iterator (12), thrust::make_constant_iterator (12) + depth_width * depth_height, @@ -354,23 +349,23 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, #if 1 typename Storage::type disp_helper_map (output_size); - transform (counting_iterator(0), - counting_iterator(0) + output_size, + transform (thrust::counting_iterator(0), + thrust::counting_iterator(0) + output_size, disp_helper_map.begin (), DisparityHelperMap (thrust::raw_pointer_cast(&depth[0]), output->width, output->height, smoothing_filter_size, baseline, 1.0f/constant, disp_thresh)); for (int iter = 0; iter < smoothing_nr_iterations; iter++) { transform ( - make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))) + output_size, - depth.begin (), DisparityClampedSmoothing (raw_pointer_cast(&depth[0]), raw_pointer_cast(&disp_helper_map[0]), output->width, output->height, smoothing_filter_size)); + thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))) + output_size, + depth.begin (), DisparityClampedSmoothing (thrust::raw_pointer_cast(&depth[0]), thrust::raw_pointer_cast(&disp_helper_map[0]), output->width, output->height, smoothing_filter_size)); } // Send the data to the device transform ( - make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator(0))) + output_size, + thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), thrust::counting_iterator(0))) + output_size, output->points.begin (), ComputeXYZRGB (output->width, output->height, output->width >> 1, output->height >> 1, constant)); @@ -378,19 +373,19 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, // typename Storage::type smooth_depth1 (output_size); // typename Storage::type smooth_depth2 (output_size); // -// transform (counting_iterator(0), -// counting_iterator(0) + output_size, +// transform (thrust::counting_iterator(0), +// thrust::counting_iterator(0) + output_size, // smooth_depth1.begin (), // DisparityBoundSmoothing (output->width, output->height, smoothing_filter_size, 1.0f/constant, baseline, disp_thresh, thrust::raw_pointer_cast(&depth[0]), thrust::raw_pointer_cast(&depth[0]))); // // for (int iter = 0; iter < (smoothing_nr_iterations-1)/2; iter++) // { -// transform (counting_iterator(0), -// counting_iterator(0) + output_size, +// transform (thrust::counting_iterator(0), +// thrust::counting_iterator(0) + output_size, // smooth_depth2.begin (), // DisparityBoundSmoothing (output->width, output->height, smoothing_filter_size, 1.0f/constant, baseline, disp_thresh, thrust::raw_pointer_cast(&smooth_depth1[0]), thrust::raw_pointer_cast(&depth[0]))); -// transform (counting_iterator(0), -// counting_iterator(0) + output_size, +// transform (thrust::counting_iterator(0), +// thrust::counting_iterator(0) + output_size, // smooth_depth1.begin (), // DisparityBoundSmoothing (output->width, output->height, smoothing_filter_size, 1.0f/constant, baseline, disp_thresh, thrust::raw_pointer_cast(&smooth_depth2[0]), thrust::raw_pointer_cast(&depth[0]))); // } @@ -399,8 +394,8 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, // // // Send the data to the device // transform ( -// make_zip_iterator (make_tuple (smooth_depth1.begin (), rgb.begin (), counting_iterator(0))), -// make_zip_iterator (make_tuple (smooth_depth1.begin (), rgb.begin (), counting_iterator(0))) + output_size, +// thrust::make_zip_iterator (make_tuple (smooth_depth1.begin (), rgb.begin (), thrust::counting_iterator(0))), +// thrust::make_zip_iterator (make_tuple (smooth_depth1.begin (), rgb.begin (), thrust::counting_iterator(0))) + output_size, // output->points.begin (), // ComputeXYZRGB (output->width, output->height, // output->width >> 1, output->height >> 1, constant)); @@ -410,8 +405,8 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, { // Send the data to the device transform ( - make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), counting_iterator(0))) + output_size, + thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin (), rgb.begin (), thrust::counting_iterator(0))) + output_size, output->points.begin (), ComputeXYZRGB (output->width, output->height, output->width >> 1, output->height >> 1, constant)); @@ -421,8 +416,8 @@ DisparityToCloud::compute (const openni_wrapper::DepthImage::Ptr& depth_image, { // Send the data to the device transform ( - make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))), - make_zip_iterator (make_tuple (depth.begin (), counting_iterator(0))) + + thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))), + thrust::make_zip_iterator (make_tuple (depth.begin (), thrust::counting_iterator(0))) + output->width * output->height, output->points.begin (), ComputeXYZ (output->width, output->height, diff --git a/cuda/io/src/kinect_smoothing.cu b/cuda/io/src/kinect_smoothing.cu index 3851442e..c7137e22 100644 --- a/cuda/io/src/kinect_smoothing.cu +++ b/cuda/io/src/kinect_smoothing.cu @@ -93,8 +93,6 @@ namespace pcl // if (!output) // output.reset (new PointCloudAOS); // - // using namespace thrust; - // // // Prepare the output // output->height = depth_image->height; // output->width = depth_image->width; @@ -185,8 +183,6 @@ namespace pcl if (!output) output.reset (new PointCloudAOS); - using namespace thrust; - int depth_width = depth_image->getWidth (); int depth_height = depth_image->getHeight (); diff --git a/cuda/nn/organized_neighbor_search.h b/cuda/nn/organized_neighbor_search.h index f177fbe9..d1dcbab3 100644 --- a/cuda/nn/organized_neighbor_search.h +++ b/cuda/nn/organized_neighbor_search.h @@ -40,6 +40,7 @@ #include #include +#include #include #include @@ -73,9 +74,7 @@ namespace pcl /** \brief Empty deconstructor. */ virtual - ~OrganizedNeighborSearch () - { - } + ~OrganizedNeighborSearch() = default; // public typedefs using PointCloud = pcl::PointCloud; @@ -111,7 +110,7 @@ namespace pcl int radiusSearch (const PointCloudConstPtr &cloud_arg, int index_arg, double radius_arg, std::vector &k_indices_arg, std::vector &k_sqr_distances_arg, - int max_nn_arg = INT_MAX); + int max_nn_arg = std::numeric_limits::max()); /** \brief Search for all neighbors of query point that are within a given radius. * \param index_arg index representing the query point in the dataset given by \a setInputCloud. @@ -124,7 +123,7 @@ namespace pcl */ int radiusSearch (int index_arg, const double radius_arg, std::vector &k_indices_arg, - std::vector &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const; + std::vector &k_sqr_distances_arg, int max_nn_arg = std::numeric_limits::max()) const; /** \brief Search for all neighbors of query point that are within a given radius. * \param p_q_arg the given query point @@ -136,7 +135,7 @@ namespace pcl */ int radiusSearch (const PointT &p_q_arg, const double radius_arg, std::vector &k_indices_arg, - std::vector &k_sqr_distances_arg, int max_nn_arg = INT_MAX) const; + std::vector &k_sqr_distances_arg, int max_nn_arg = std::numeric_limits::max()) const; /** \brief Search for k-nearest neighbors at the query point. * \param cloud_arg the point cloud data @@ -210,11 +209,6 @@ namespace pcl { } - /** \brief Empty deconstructor */ - ~radiusSearchLoopkupEntry () - { - } - /** \brief Define search point and calculate squared distance * @param x_shift shift in x dimension * @param y_shift shift in y dimension @@ -258,11 +252,6 @@ namespace pcl { } - /** \brief Empty deconstructor */ - ~nearestNeighborCandidate () - { - } - /** \brief Operator< for comparing nearestNeighborCandidate instances with each other. */ bool operator< (const nearestNeighborCandidate& rhs_arg) const diff --git a/cuda/sample_consensus/CMakeLists.txt b/cuda/sample_consensus/CMakeLists.txt index e5a19821..42cdc30c 100644 --- a/cuda/sample_consensus/CMakeLists.txt +++ b/cuda/sample_consensus/CMakeLists.txt @@ -8,7 +8,7 @@ set(SUBSYS_DEPS cuda_common io common) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) mark_as_advanced("BUILD_${SUBSYS_NAME}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS}) +PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) PCL_SET_SUBSYS_INCLUDE_DIR("${SUBSYS_NAME}" "${SUBSYS_PATH}") if(NOT build) diff --git a/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac.h b/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac.h index 10b12619..e346f38b 100644 --- a/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac.h +++ b/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac.h @@ -39,7 +39,7 @@ #include #include -#include +#include //#include namespace pcl @@ -72,8 +72,8 @@ namespace pcl * \param model a Sample Consensus model */ SampleConsensus (const SampleConsensusModelPtr &model) : - sac_model_(model), probability_ (0.99), iterations_ (0), threshold_ (DBL_MAX), - max_iterations_ (1000) + sac_model_(model), probability_(0.99), iterations_(0), + threshold_(std::numeric_limits::max()), max_iterations_(1000) {}; /** \brief Constructor for base SAC. @@ -86,7 +86,7 @@ namespace pcl {}; /** \brief Destructor for base SAC. */ - virtual ~SampleConsensus () {}; + virtual ~SampleConsensus() = default; /** \brief Set the distance to model threshold. * \param threshold distance to model threshold diff --git a/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac_model.h b/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac_model.h index ba27c9b7..b9f6d47e 100644 --- a/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac_model.h +++ b/cuda/sample_consensus/include/pcl/cuda/sample_consensus/sac_model.h @@ -37,7 +37,7 @@ #pragma once -#include +#include #include #include #include @@ -108,7 +108,9 @@ namespace pcl private: /** \brief Empty constructor for base SampleConsensusModel. */ - SampleConsensusModel () : radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX) + SampleConsensusModel() : + radius_min_(std::numeric_limits::lowest()), + radius_max_(std::numeric_limits::max()) {}; public: @@ -116,7 +118,8 @@ namespace pcl * \param cloud the input point cloud dataset */ SampleConsensusModel (const PointCloudConstPtr &cloud) : - radius_min_ (-FLT_MAX), radius_max_ (FLT_MAX) + radius_min_(std::numeric_limits::lowest()), + radius_max_(std::numeric_limits::max()) { // Sets the input cloud and creates a vector of "fake" indices setInputCloud (cloud); @@ -129,7 +132,8 @@ namespace pcl /* SampleConsensusModel (const PointCloudConstPtr &cloud, const std::vector &indices) : input_ (cloud), indices_ (boost::make_shared > (indices)), - radius_min_ (-DBL_MAX), radius_max_ (DBL_MAX) + radius_min_ (std::numeric_limits::lowest()), + radius_max_ (std::numeric_limits::max()) { if (indices_->size () > input_->points.size ()) @@ -140,7 +144,7 @@ namespace pcl };*/ /** \brief Destructor for base SampleConsensusModel. */ - virtual ~SampleConsensusModel () {}; + virtual ~SampleConsensusModel() = default; /** \brief Get a set of random data samples and return them as point * indices. Pure virtual. diff --git a/cuda/sample_consensus/src/msac.cpp b/cuda/sample_consensus/src/msac.cpp index 73daa198..397403c5 100644 --- a/cuda/sample_consensus/src/msac.cpp +++ b/cuda/sample_consensus/src/msac.cpp @@ -38,6 +38,7 @@ #include #include #include +#include //CUPRINTF #include "cuPrintf.cu" int min_nr_in_shape = 5000; @@ -47,7 +48,7 @@ template